From 6d81743a0bb03d33719176f614972a86c0e33ac0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 18 Oct 2018 11:40:41 +0200 Subject: [PATCH 01/14] comments --- visualization/motion_planning_tasks/src/factory_model.h | 3 +++ .../motion_planning_tasks/src/pluginlib_factory.h | 8 +++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/visualization/motion_planning_tasks/src/factory_model.h b/visualization/motion_planning_tasks/src/factory_model.h index 2456c0df..01c83b86 100644 --- a/visualization/motion_planning_tasks/src/factory_model.h +++ b/visualization/motion_planning_tasks/src/factory_model.h @@ -41,6 +41,9 @@ namespace moveit_rviz_plugin { +/** Provide a tree model listing all available plugins from the rviz::Factory + * grouped by package name + */ class FactoryModel : public QStandardItemModel { QString mime_type_; diff --git a/visualization/motion_planning_tasks/src/pluginlib_factory.h b/visualization/motion_planning_tasks/src/pluginlib_factory.h index 582f9069..58a8783d 100644 --- a/visualization/motion_planning_tasks/src/pluginlib_factory.h +++ b/visualization/motion_planning_tasks/src/pluginlib_factory.h @@ -54,6 +54,9 @@ namespace moveit_rviz_plugin { +/** Templated factory to create objects of a given pluginlib base class type. + * This is a slightly modified version of rviz::PluginlibFactory, providing a custom mime type. + */ template class PluginlibFactory: public rviz::Factory { @@ -177,9 +180,8 @@ public: * @param error_return If non-NULL and there is an error, *error_return is set to a description of the problem. * @return A new instance of the class identified by class_id, or NULL if there was an error. * - * If makeRaw() returns NULL and error_return is not NULL, - * *error_return will be set. On success, *error_return will not be - * changed. */ + * If makeRaw() returns NULL and error_return is not NULL, *error_return will be set. + * On success, *error_return will not be changed. */ virtual Type* makeRaw( const QString& class_id, QString* error_return = NULL ) { typename QHash::const_iterator iter = built_ins_.find( class_id ); From 5b8d841094978baff41683341c43048901425c48 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Oct 2018 12:39:53 +0100 Subject: [PATCH 02/14] RemoteTaskModel: show stage properties --- .../properties/property_factory.cpp | 15 ++++++++ .../properties/property_factory.h | 7 ++-- .../src/remote_task_model.cpp | 36 ++++++++++++++++++- 3 files changed, 55 insertions(+), 3 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index ed5bc290..4cab4add 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -81,6 +81,21 @@ rviz::Property* PropertyFactory::create(const std::string& prop_name, Property* return it->second(QString::fromStdString(prop_name), prop); } +rviz::Property* PropertyFactory::create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const +{ + if (old) { // reuse existing Property? + old->setDescription(QString::fromStdString(p.description)); + old->setValue(QString::fromStdString(p.value)); + return old; + } else { // create new Property? + rviz::Property *result = new rviz::StringProperty(QString::fromStdString(p.name), + QString::fromStdString(p.value), + QString::fromStdString(p.description)); + result->setReadOnly(true); + return result; + } +} + rviz::PropertyTreeModel* createPropertyTreeModel(PropertyMap& properties, QObject* parent) { PropertyFactory& factory = PropertyFactory::instance(); diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index 6176445b..1bd98486 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -41,6 +41,7 @@ #include #include #include +#include namespace rviz { class Property; @@ -62,10 +63,12 @@ public: /// register a new factory function for type T template - void registerType(const FactoryFunction& f) { registerType(std::type_index(typeid(T)).name(), f); } + void registerType(const FactoryFunction& f) { registerType(typeid(T).name(), f); } - /// retrieve rviz property for given task_constructor property + /// create rviz::Property for given MTC Property rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property *prop) const; + /// create rviz::Property for given MTC property message + rviz::Property* create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const; private: std::map registry_; diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 0cbd04bf..0c757c4c 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -37,11 +37,12 @@ #include #include "remote_task_model.h" +#include "properties/property_factory.h" #include #include #include #include -#include +#include #include #include @@ -78,8 +79,39 @@ struct RemoteTaskModel::Node { name_ = name; return true; } + + void setProperties(const std::vector& props); }; +void RemoteTaskModel::Node::setProperties(const std::vector &props) +{ + // insert properties in same order as reported in description + rviz::Property *root = properties_->getRoot(); + int index = 0; // current child index in root + for (auto it = props.begin(); it != props.end(); ++it) { + int num = root->numChildren(); + // find first child with name >= it->name + int next = index; + while (next < num && root->childAt(next)->getName().toStdString() < it->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().toStdString() != it->name) + old_child = nullptr; + + rviz::Property *new_child = PropertyFactory::instance().create(*it, old_child); + if (new_child != old_child) + root->addChild(new_child, index); + ++index; + } + // remove remaining children + root->removeChildren(index, root->numChildren()-index); +} + // return Node* corresponding to index RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex &index) const { @@ -260,6 +292,8 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg if (!(n->node_flags_ & NAME_CHANGED)) // avoid overwriting a manually changed name changed |= n->setName(QString::fromStdString(s.name)); + n->setProperties(s.properties); + InterfaceFlags old_flags = n->interface_flags_; n->interface_flags_ = InterfaceFlags(); for (auto f : {READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END}) { From 062cfe0b609d42cf9bc8809d23b36f76c5e4883a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Nov 2018 21:21:19 +0100 Subject: [PATCH 03/14] PropertyTypeRegistry to store serialization/deserialization functions --- .../moveit/task_constructor/properties.h | 167 +++++++++++++----- core/src/introspection.cpp | 1 + core/src/properties.cpp | 128 ++++++++++++-- core/src/stages/move_relative.cpp | 3 + core/src/stages/move_to.cpp | 5 + core/test/test_properties.cpp | 14 +- msgs/msg/Property.msg | 1 + 7 files changed, 256 insertions(+), 63 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 22112045..5e910537 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -45,18 +45,11 @@ #include #include #include +#include namespace moveit { namespace task_constructor { -// hasSerialize::value provides a true/false constexpr depending on whether operator<< is supported. -// This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html -template -struct hasSerialize : std::false_type {}; - -template -struct hasSerialize() << std::declval())> : std::true_type {}; - class Property; class PropertyMap; @@ -78,25 +71,8 @@ class Property { friend class PropertyMap; typedef decltype(std::declval().type()) type_index; - typedef std::function SerializeFunction; - - Property(const type_index &type_index, const std::string &description, const boost::any &default_value, - const Property::SerializeFunction &serialize); - - template - static typename std::enable_if::value, std::string>::type - serialize(const boost::any& value) { - if (value.empty()) return ""; - std::ostringstream oss; - oss << boost::any_cast(value); - return oss.str(); - } - - template - static typename std::enable_if::value, std::string>::type - serialize(const boost::any& value) { - return ""; - } + /// typed constructor is only accessible via PropertyMap + Property(const type_index &type_index, const std::string &description, const boost::any &default_value); public: /// base class for Property exceptions @@ -111,8 +87,6 @@ public: typedef uint SourceFlags; /// function callback used to initialize property value from another PropertyMap typedef std::function InitializerFunction; - /// function callback used to signal value setting to external components - typedef std::function SignalFunction; /// set current value and default value void setValue(const boost::any& value); @@ -128,8 +102,12 @@ public: inline const boost::any& value() const { return value_.empty() ? default_ : value_; } /// get default value const boost::any& defaultValue() const { return default_; } - /// serialize current value - std::string serialize(const boost::any& value) const; + + /// serialize/print value using registered functions + static std::string print(const boost::any& value); + static std::string serialize(const boost::any& value); + static boost::any deserialize(const std::string& type_name, const std::string& wire); + std::string print() const { return print(value()); } std::string serialize() const { return serialize(value()); } /// get description text @@ -137,7 +115,8 @@ public: void setDescription(const std::string& desc) { description_ = desc; } /// get typename - std::string typeName() const { return type_index_.name(); } + static std::string typeName(const std::type_index& type_index); + std::string typeName() const; /// return true, if property initialized from given SourceId bool initsFrom(SourceFlags source) const; @@ -146,10 +125,6 @@ public: /// configure initialization from source using given other property name Property &configureInitFrom(SourceFlags source, const std::string& name); - /// define a function callback to be called on each value update - /// note, that boost::any doesn't allow for change detection - void setSignalCallback(const SignalFunction& f) { signaller_ = f; } - private: std::string description_; type_index type_index_; @@ -160,8 +135,6 @@ private: SourceFlags source_flags_ = 0; SourceFlags initialized_from_; InitializerFunction initializer_; - SignalFunction signaller_; - SerializeFunction serialize_; }; @@ -189,6 +162,114 @@ public: }; +// hasSerialize::value provides a true/false constexpr depending on whether operator<< is supported. +// This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html +template +struct hasSerialize : std::false_type {}; + +template +struct hasSerialize() << std::declval())> : std::true_type {}; + +template +struct hasDeserialize : std::false_type {}; + +template +struct hasDeserialize() >> std::declval())> : std::true_type {}; + +class PropertySerializerBase { +public: + typedef std::string (*SerializeFunction)(const boost::any&); + typedef boost::any (*DeserializeFunction)(const std::string&); + typedef std::string (*PrintFunction)(const boost::any&); + + static std::string dummySerialize(const boost::any&) { return ""; } + static boost::any dummyDeserialize(const std::string&) { return boost::any(); } + +protected: + static bool insert(const std::type_index& type_index, const std::string& type_name, + SerializeFunction serialize, DeserializeFunction deserialize, PrintFunction print); +}; + +/// utility class to register serializer/deserializer functions for a property of type T +template +class PropertySerializer : protected PropertySerializerBase { +public: + PropertySerializer() { + insert(typeid(T), typeName(), &serialize, &deserialize, &print); + } + + template + static typename std::enable_if::value, std::string>::type + typeName() { + return ros::message_traits::DataType::value(); + } + + template + static typename std::enable_if::value, std::string>::type + typeName() { return typeid(T).name(); } + +private: + /** ROS message serialization */ + template + static typename std::enable_if::value, std::string>::type + serialize(const boost::any& value) { + static_assert(sizeof(uint8_t) == sizeof(char), "Assuming char has same size as uint8_t"); + const T& msg = boost::any_cast(value); + std::size_t size = ros::serialization::serializationLength(msg); + std::string result(size, '\0'); + if (size) + { + ros::serialization::OStream stream(reinterpret_cast(&result[0]), size); + ros::serialization::serialize(stream, msg); + } + return result; + } + template + static typename std::enable_if::value, boost::any>::type + deserialize(const std::string& wire) { + T value; + ros::serialization::IStream stream(const_cast(reinterpret_cast(&wire[0])), wire.size()); + ros::serialization::deserialize(stream, value); + return value; + } + + /** Serialization based on std::[io]stringstream */ + template + static typename std::enable_if::value, std::string>::type + print(const boost::any& value) { + std::ostringstream oss; + oss << boost::any_cast(value); + return oss.str(); + } + + template + static typename std::enable_if::value && hasSerialize::value, std::string>::type + serialize(const boost::any& value) { + return print(value); + } + template + static typename std::enable_if::value && hasSerialize::value && hasDeserialize::value, boost::any>::type + deserialize(const std::string& wired) { + std::istringstream iss(wired); + T value; + iss >> value; + return value; + } + + /** No serialization available */ + template + static typename std::enable_if::value, std::string>::type + print(const boost::any& value) { return dummySerialize(value); } + + template + static typename std::enable_if::value && !hasSerialize::value, std::string>::type + serialize(const boost::any& value) { return dummySerialize(value); } + template + static typename std::enable_if::value && (!hasSerialize::value || !hasDeserialize::value), boost::any>::type + deserialize(const std::string& wire) { return dummyDeserialize(wire); } +}; + + /** PropertyMap is map of (name, Property) pairs. * * Conveniency methods are provided to setup property initialization for several @@ -202,20 +283,20 @@ class PropertyMap /// implementation of declare methods Property& declare(const std::string& name, const Property::type_index& type_index, - const std::string& description, - const boost::any& default_value, - const Property::SerializeFunction &serialize); + const std::string& description, const boost::any& default_value); public: /// declare a property for future use template Property& declare(const std::string& name, const std::string& description = "") { - return declare(name, typeid(T), description, boost::any(), &Property::serialize); + PropertySerializer(); // register serializer/deserializer + return declare(name, typeid(T), description, boost::any()); } /// declare a property with default value template Property& declare(const std::string& name, const T& default_value, const std::string& description = "") { - return declare(name, typeid(T), description, default_value, &Property::serialize); + PropertySerializer(); // register serializer/deserializer + return declare(name, typeid(T), description, default_value); } /// declare all given properties also in other PropertyMap diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 52b0d45e..fa6f1615 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -227,6 +227,7 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio moveit_task_constructor_msgs::Property p; p.name = pair.first; p.description = pair.second.description(); + p.type = pair.second.typeName(); p.value = pair.second.serialize(); desc.properties.push_back(p); } diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 43c520c6..3205e82a 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -44,14 +44,83 @@ namespace moveit { namespace task_constructor { -Property::Property(const Property::type_index& type_index, const std::string& description, const boost::any& default_value, - const Property::SerializeFunction &serialize) +const std::string LOGNAME = "Properties"; + +class PropertyTypeRegistry { + struct Entry { + std::string name_; + PropertySerializerBase::SerializeFunction serialize_; + PropertySerializerBase::DeserializeFunction deserialize_; + PropertySerializerBase::PrintFunction print_; + }; + Entry dummy_; + + // map from type_info to corresponding converter functions + typedef std::map RegistryMap; + RegistryMap types_; + // map from type names (type.name or ROS msg name) to entry in types_ + typedef std::map TypeNameMap; + TypeNameMap names_; + +public: + PropertyTypeRegistry() : dummy_{"", PropertySerializerBase::dummySerialize, + PropertySerializerBase::dummyDeserialize, + PropertySerializerBase::dummySerialize} + {} + inline bool insert(const std::type_index& type_index, const std::string& type_name, + PropertySerializerBase::SerializeFunction serialize, + PropertySerializerBase::DeserializeFunction deserialize, + PropertySerializerBase::PrintFunction print); + + const Entry& entry(const std::type_index& type_index) { + auto it = types_.find(type_index); + if (it == types_.end()) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Unregistered type: " << type_index.name()); + return dummy_; + } + return it->second; + } + const Entry& entry(const std::string& type_name) const { + auto it = names_.find(type_name); + if (it == names_.end()) + return dummy_; + return it->second->second; + } +}; +static PropertyTypeRegistry registry_singleton_; + +bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std::string& type_name, + PropertySerializerBase::SerializeFunction serialize, + PropertySerializerBase::DeserializeFunction deserialize, + PropertySerializerBase::PrintFunction print) +{ + if (type_index == std::type_index(typeid(boost::any))) + return false; + + auto it_inserted = types_.insert(std::make_pair(type_index, Entry {type_name, serialize, deserialize, print})); + if (!it_inserted.second) + return false; // was already registered before + + if (!type_name.empty()) // register type_name too? + names_.insert(std::make_pair(type_name, it_inserted.first)); + + return true; +} + +bool PropertySerializerBase::insert(const std::type_index& type_index, const std::string& type_name, + PropertySerializerBase::SerializeFunction serialize, + PropertySerializerBase::DeserializeFunction deserialize, PrintFunction print) +{ + return registry_singleton_.insert(type_index, type_name, serialize, deserialize, print); +} + + +Property::Property(const Property::type_index& type_index, const std::string& description, const boost::any& default_value) : description_(description) , type_index_(type_index) , default_(default_value) , value_() , initialized_from_(-1) - , serialize_(serialize) { // default value's type should match declared type by construction assert(default_.empty() || default_.type() == type_index_ || type_index_ == typeid(boost::any)); @@ -71,9 +140,6 @@ void Property::setCurrentValue(const boost::any &value) value_ = value; initialized_from_ = 1; // manually initialized TODO: use enums - - if (signaller_) - signaller_(this); } void Property::reset() @@ -84,9 +150,39 @@ void Property::reset() initialized_from_ = -1; // set to max value } -std::string Property::serialize(const boost::any& v) const { - if (!serialize_ || v.empty()) return ""; - return serialize_(v); +std::string Property::serialize(const boost::any& value) +{ + if (value.empty()) + return ""; + return registry_singleton_.entry(value.type()).serialize_(value); +} + +boost::any Property::deserialize(const std::string& type_name, const std::string& wire) +{ + if (wire.empty()) + return boost::any(); + else + return registry_singleton_.entry(type_name).deserialize_(wire); +} + +std::string Property::print(const boost::any& value) +{ + if (value.empty()) + return ""; + return registry_singleton_.entry(value.type()).print_(value); +} + +std::string Property::typeName() const { + if (value().empty()) return typeName(type_index_); + else return typeName(value().type()); +} + +std::string Property::typeName(const std::type_index& type_index) +{ + if (type_index == typeid(boost::any)) + return ""; + else + return registry_singleton_.entry(type_index).name_; } bool Property::initsFrom(Property::SourceFlags source) const @@ -111,10 +207,9 @@ Property &Property::configureInitFrom(SourceFlags source, const std::string &nam Property& PropertyMap::declare(const std::string &name, const Property::type_index &type_index, - const std::string &description, const boost::any &default_value, - const Property::SerializeFunction &serialize) + const std::string &description, const boost::any &default_value) { - auto it_inserted = props_.insert(std::make_pair(name, Property(type_index, description, default_value, serialize))); + auto it_inserted = props_.insert(std::make_pair(name, Property(type_index, description, default_value))); // if name was already declared, the new declaration should match in type (except it was boost::any) if (!it_inserted.second && it_inserted.first->second.type_index_ != typeid(boost::any) && type_index != it_inserted.first->second.type_index_) @@ -145,7 +240,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::set &prop void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const { const Property& p = property(name); - other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); + other.declare(other_name, p.type_index_, p.description_, p.default_); } void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set &properties) @@ -167,8 +262,7 @@ void PropertyMap::set(const std::string& name, const boost::any& val if (range.first == range.second) { // name is not yet declared if (value.empty()) throw Property::undeclared(name, "trying to set undeclared property '" + name + "' with NULL value"); - auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any(), - Property::SerializeFunction()))); + auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any()))); it->second.setValue(value); } else range.first->second.setValue(value); @@ -221,8 +315,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } catch (const Property::undefined&) { } - ROS_DEBUG_STREAM_NAMED("Properties", pair.first << ": " << p.initialized_from_ << - " -> " << source << ": " << p.serialize(value)); + ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << + " -> " << source << ": " << Property::print(value)); p.setCurrentValue(value); p.initialized_from_ = source; } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index f12ce013..6520764f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -53,6 +53,9 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf p.declare("ik_frame", "frame to be moved in Cartesian direction"); p.declare("direction", "motion specification"); + // register actual types + PropertySerializer(); + PropertySerializer(); p.declare("min_distance", -1.0, "minimum distance to move"); p.declare("max_distance", 0.0, "maximum distance to move"); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index a1db9f3e..e9302fb0 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -53,6 +53,11 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan p.declare("group", "name of planning group"); p.declare("ik_frame", "frame to be moved towards goal pose"); p.declare("goal", "goal specification"); + // register actual types + PropertySerializer(); + PropertySerializer(); + PropertySerializer(); + PropertySerializer(); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 6bc43c42..17785d30 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -27,12 +27,11 @@ TEST(Property, directset) { PropertyMap props; props.set("int1", 1); EXPECT_EQ(props.get("int1"), 1); - EXPECT_STREQ(props.property("int1").serialize().c_str(), "1"); + EXPECT_EQ(props.property("int1").serialize(), "1"); props.set("int2", boost::any(2)); EXPECT_EQ(props.get("int2"), 2); - // cannot serialize, because directly set - EXPECT_STREQ(props.property("int2").serialize().c_str(), ""); + EXPECT_EQ(props.property("int2").serialize(), "2"); } TEST(Property, redeclare) { @@ -87,6 +86,15 @@ TEST(Property, anytype) { EXPECT_EQ(props.get("any"), "foo"); } +TEST(Property, serialize_basic) { + EXPECT_TRUE(hasSerialize::value); + EXPECT_TRUE(hasDeserialize::value); + EXPECT_TRUE(hasSerialize::value); + EXPECT_TRUE(hasDeserialize::value); + EXPECT_TRUE(hasSerialize::value); + EXPECT_TRUE(hasDeserialize::value); +} + TEST(Property, serialize) { PropertyMap props; props.declare("int"); diff --git a/msgs/msg/Property.msg b/msgs/msg/Property.msg index e8baad4f..fb9e3275 100644 --- a/msgs/msg/Property.msg +++ b/msgs/msg/Property.msg @@ -1,3 +1,4 @@ string name string description +string type string value From 81cc0aecc6c44ac0cbba3d2833d14a9d1f9340bc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Nov 2018 21:33:29 +0100 Subject: [PATCH 04/14] Property::type_index -> boost::typeindex::type_info avoid decl expressions, explicit public typedef --- .../moveit/task_constructor/properties.h | 15 +++++---- core/src/properties.cpp | 32 +++++++++---------- 2 files changed, 25 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 5e910537..83f8a8ae 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -70,11 +70,14 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name); class Property { friend class PropertyMap; - typedef decltype(std::declval().type()) type_index; /// typed constructor is only accessible via PropertyMap - Property(const type_index &type_index, const std::string &description, const boost::any &default_value); + Property(const boost::typeindex::type_info& type_info, + const std::string &description, + const boost::any &default_value); public: + typedef boost::typeindex::type_info type_info; + /// base class for Property exceptions class error; /// exception thrown when accessing an undeclared property @@ -115,7 +118,7 @@ public: void setDescription(const std::string& desc) { description_ = desc; } /// get typename - static std::string typeName(const std::type_index& type_index); + static std::string typeName(const type_info& type_info); std::string typeName() const; /// return true, if property initialized from given SourceId @@ -127,7 +130,7 @@ public: private: std::string description_; - type_index type_index_; + const type_info& type_info_; boost::any default_; boost::any value_; @@ -282,7 +285,7 @@ class PropertyMap typedef std::map::const_iterator const_iterator; /// implementation of declare methods - Property& declare(const std::string& name, const Property::type_index& type_index, + Property& declare(const std::string& name, const Property::type_info& type_info, const std::string& description, const boost::any& default_value); public: /// declare a property for future use @@ -294,7 +297,7 @@ public: /// declare a property with default value template Property& declare(const std::string& name, const T& default_value, - const std::string& description = "") { + const std::string& description = "") { PropertySerializer(); // register serializer/deserializer return declare(name, typeid(T), description, default_value); } diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 3205e82a..a4b34a49 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -72,7 +72,7 @@ public: PropertySerializerBase::DeserializeFunction deserialize, PropertySerializerBase::PrintFunction print); - const Entry& entry(const std::type_index& type_index) { + const Entry& entry(const std::type_index& type_index) const { auto it = types_.find(type_index); if (it == types_.end()) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Unregistered type: " << type_index.name()); @@ -115,15 +115,15 @@ bool PropertySerializerBase::insert(const std::type_index& type_index, const std } -Property::Property(const Property::type_index& type_index, const std::string& description, const boost::any& default_value) +Property::Property(const type_info& type_info, const std::string& description, const boost::any& default_value) : description_(description) - , type_index_(type_index) + , type_info_(type_info) , default_(default_value) , value_() , initialized_from_(-1) { // default value's type should match declared type by construction - assert(default_.empty() || default_.type() == type_index_ || type_index_ == typeid(boost::any)); + assert(default_.empty() || default_.type() == type_info_ || type_info_ == typeid(boost::any)); reset(); } @@ -135,8 +135,8 @@ void Property::setValue(const boost::any &value) { void Property::setCurrentValue(const boost::any &value) { - if (!value.empty() && type_index_ != typeid(boost::any) && value.type() != type_index_) - throw Property::type_error(value.type().name(), type_index_.name()); + if (!value.empty() && type_info_ != typeid(boost::any) && value.type() != type_info_) + throw Property::type_error(value.type().name(), type_info_.name()); value_ = value; initialized_from_ = 1; // manually initialized TODO: use enums @@ -173,16 +173,16 @@ std::string Property::print(const boost::any& value) } std::string Property::typeName() const { - if (value().empty()) return typeName(type_index_); + if (value().empty()) return typeName(type_info_); else return typeName(value().type()); } -std::string Property::typeName(const std::type_index& type_index) +std::string Property::typeName(const type_info& type_info) { - if (type_index == typeid(boost::any)) + if (type_info == typeid(boost::any)) return ""; else - return registry_singleton_.entry(type_index).name_; + return registry_singleton_.entry(type_info).name_; } bool Property::initsFrom(Property::SourceFlags source) const @@ -206,14 +206,14 @@ Property &Property::configureInitFrom(SourceFlags source, const std::string &nam } -Property& PropertyMap::declare(const std::string &name, const Property::type_index &type_index, +Property& PropertyMap::declare(const std::string &name, const Property::type_info& type_info, const std::string &description, const boost::any &default_value) { - auto it_inserted = props_.insert(std::make_pair(name, Property(type_index, description, default_value))); + auto it_inserted = props_.insert(std::make_pair(name, Property(type_info, description, default_value))); // if name was already declared, the new declaration should match in type (except it was boost::any) - if (!it_inserted.second && it_inserted.first->second.type_index_ != typeid(boost::any) && - type_index != it_inserted.first->second.type_index_) - throw Property::type_error(type_index.name(), it_inserted.first->second.type_index_.name()); + if (!it_inserted.second && it_inserted.first->second.type_info_ != typeid(boost::any) && + type_info != it_inserted.first->second.type_info_) + throw Property::type_error(type_info.name(), it_inserted.first->second.type_info_.name()); return it_inserted.first->second; } @@ -240,7 +240,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::set &prop void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const { const Property& p = property(name); - other.declare(other_name, p.type_index_, p.description_, p.default_); + other.declare(other_name, p.type_info_, p.description_, p.default_); } void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set &properties) From 3f3c2f6bf9f35b922a7bbaa11c3773edb191d89e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 23 Nov 2018 13:10:00 +0100 Subject: [PATCH 05/14] Update MTC property on changes of rviz property --- .../properties/property_factory.cpp | 45 ++++++++++++------- .../properties/property_factory.h | 17 +++---- .../src/local_task_model.cpp | 4 +- 3 files changed, 41 insertions(+), 25 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 4cab4add..f84083a5 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -35,6 +35,7 @@ /* Author: Robert Haschke */ #include "property_factory.h" + #include #include @@ -42,25 +43,35 @@ #include #include -using namespace moveit::task_constructor; +namespace mtc = ::moveit::task_constructor; namespace moveit_rviz_plugin { -/// TODO: We also need to provide methods to sync both properties in both directions. -/// In our Property we could store a callback function to update the rviz::Property. -/// The rviz::Property can simply use a lambda-function slot to update our Property. -/// However, we need to avoid infinite loops in doing so. Our Property cannot compare values... -template -RVIZProp* helper(const QString& name, Property* prop) { - T value = prop->defined() ? boost::any_cast(prop->value()) : T(); - return new RVIZProp(name, value, QString::fromStdString(prop->description())); +static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop) { + std::string value; + if (mtc_prop.defined()) + value = boost::any_cast(mtc_prop.value()); + rviz::StringProperty* rviz_prop = new rviz::StringProperty(name, QString::fromStdString(value), + QString::fromStdString(mtc_prop.description())); + QObject::connect(rviz_prop, &rviz::StringProperty::changed, + [rviz_prop, &mtc_prop]() {mtc_prop.setValue(rviz_prop->getStdString());}); + return rviz_prop; +} +template +static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop) { + T value = mtc_prop.defined() ? boost::any_cast(mtc_prop.value()) : T(); + rviz::FloatProperty* rviz_prop = new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); + QObject::connect(rviz_prop, &rviz::FloatProperty::changed, + [rviz_prop, &mtc_prop]() {mtc_prop.setValue(T(rviz_prop->getFloat()));}); + return rviz_prop; } PropertyFactory::PropertyFactory() { - // registe some standard types - registerType(&helper); - registerType(&helper); + // register some standard types + registerType(&floatFactory); + registerType(&floatFactory); + registerType(&stringFactory); } PropertyFactory& PropertyFactory::instance() @@ -71,12 +82,14 @@ PropertyFactory& PropertyFactory::instance() void PropertyFactory::registerType(const std::string &type_name, const FactoryFunction &f) { + if (type_name.empty()) + return; registry_.insert(std::make_pair(type_name, f)); } -rviz::Property* PropertyFactory::create(const std::string& prop_name, Property* prop) const +rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop) const { - auto it = registry_.find(prop->typeName()); + auto it = registry_.find(prop.typeName()); if (it == registry_.end()) return nullptr; return it->second(QString::fromStdString(prop_name), prop); } @@ -96,13 +109,13 @@ rviz::Property* PropertyFactory::create(const moveit_task_constructor_msgs::Prop } } -rviz::PropertyTreeModel* createPropertyTreeModel(PropertyMap& properties, QObject* parent) { +rviz::PropertyTreeModel* defaultPropertyTreeModel(mtc::PropertyMap& properties, QObject* parent) { PropertyFactory& factory = PropertyFactory::instance(); rviz::Property* root = new rviz::Property(); rviz::PropertyTreeModel *model = new rviz::PropertyTreeModel(root, parent); for (auto& prop : properties) { - rviz::Property* rviz_prop = factory.create(prop.first, &prop.second); + rviz::Property* rviz_prop = factory.create(prop.first, prop.second); if (!rviz_prop) rviz_prop = new rviz::Property(QString::fromStdString(prop.first)); rviz_prop->setParent(root); } diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index 1bd98486..ca503b07 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -41,16 +41,14 @@ #include #include #include + +#include #include namespace rviz { class Property; class PropertyTreeModel; } -namespace moveit { namespace task_constructor { -class Property; -class PropertyMap; -} } namespace moveit_rviz_plugin { @@ -59,14 +57,17 @@ class PropertyFactory public: static PropertyFactory& instance(); - typedef std::function FactoryFunction; + typedef std::function FactoryFunction; /// register a new factory function for type T template - void registerType(const FactoryFunction& f) { registerType(typeid(T).name(), f); } + inline void registerType(const FactoryFunction& f) { + moveit::task_constructor::PropertySerializer(); // register serializer + registerType(moveit::task_constructor::Property::typeName(typeid(T)), f); + } /// create rviz::Property for given MTC Property - rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property *prop) const; + rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property &prop) const; /// create rviz::Property for given MTC property message rviz::Property* create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const; @@ -82,6 +83,6 @@ private: }; /// turn a PropertyMap into an rviz::PropertyTreeModel -rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::PropertyMap &properties, QObject *parent = nullptr); +rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap &properties, QObject *parent = nullptr); } diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index a7a758a4..cbf3a945 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -245,8 +245,10 @@ rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &ind Node *n = node(index); if (!n) return nullptr; auto it_inserted = properties_.insert(std::make_pair(n, nullptr)); + // TODO: We might need custom factory methods to create PropertyTreeModels + // that are specifically tailored to a Stage class. if (it_inserted.second) // newly inserted, create new model - it_inserted.first->second = createPropertyTreeModel(n->me()->properties(), this); + it_inserted.first->second = defaultPropertyTreeModel(n->me()->properties(), this); return it_inserted.first->second; } From 780e09145e98cc0f1beb6cdeb957aa93de685788 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Jan 2019 20:30:13 +0100 Subject: [PATCH 06/14] stage-specific factories for PropertyTreeModel To allow stages to display their properties in a custom fashion, e.g. to show the end-effector marker for PoseStamped, we need to be able to register stage-specific factories. --- .../properties/property_factory.cpp | 56 ++++++++++++++----- .../properties/property_factory.h | 32 ++++++++--- .../src/local_task_model.cpp | 7 ++- 3 files changed, 71 insertions(+), 24 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index f84083a5..52ccb933 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -37,6 +37,7 @@ #include "property_factory.h" #include +#include #include #include @@ -80,17 +81,22 @@ PropertyFactory& PropertyFactory::instance() return instance_; } -void PropertyFactory::registerType(const std::string &type_name, const FactoryFunction &f) +void PropertyFactory::registerType(const std::string &type_name, const PropertyFactoryFunction &f) { if (type_name.empty()) return; - registry_.insert(std::make_pair(type_name, f)); + property_registry_.insert(std::make_pair(type_name, f)); +} + +void PropertyFactory::registerStage(const std::type_index &type_index, const PropertyFactory::TreeFactoryFunction &f) +{ + stage_registry_.insert(std::make_pair(type_index, f)); } rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop) const { - auto it = registry_.find(prop.typeName()); - if (it == registry_.end()) return nullptr; + auto it = property_registry_.find(prop.typeName()); + if (it == property_registry_.end()) return nullptr; return it->second(QString::fromStdString(prop_name), prop); } @@ -109,20 +115,42 @@ rviz::Property* PropertyFactory::create(const moveit_task_constructor_msgs::Prop } } -rviz::PropertyTreeModel* defaultPropertyTreeModel(mtc::PropertyMap& properties, QObject* parent) { - PropertyFactory& factory = PropertyFactory::instance(); +rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage &stage) +{ + auto it = stage_registry_.find(typeid(stage)); + if (it == stage_registry_.end()) + return defaultPropertyTreeModel(stage.properties()); + return it->second(stage.properties()); +} - rviz::Property* root = new rviz::Property(); - rviz::PropertyTreeModel *model = new rviz::PropertyTreeModel(root, parent); - for (auto& prop : properties) { - rviz::Property* rviz_prop = factory.create(prop.first, prop.second); - if (!rviz_prop) rviz_prop = new rviz::Property(QString::fromStdString(prop.first)); - rviz_prop->setParent(root); +rviz::PropertyTreeModel* PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties) { + auto root = new rviz::Property(); + addRemainingProperties(root, properties); + return new rviz::PropertyTreeModel(root, nullptr); +} + +static bool hasChild(rviz::Property* root, const QString& name) { + for (int i = 0, end = root->numChildren(); i != end; ++i) { + if (root->childAt(i)->getName() == name) + return true; } + return false; +} + +void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::PropertyMap& properties) { + for (auto& prop : properties) { + const QString& name = QString::fromStdString(prop.first); + if (hasChild(root, name)) + continue; + + rviz::Property* rviz_prop = create(prop.first, prop.second); + if (!rviz_prop) rviz_prop = new rviz::Property(name); + root->addChild(rviz_prop); + } + // just to see something, when no properties are defined - if (model->rowCount() == 0) + if (root->numChildren() == 0) new rviz::Property("no properties", QVariant(), QString(), root); - return model; } } diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index ca503b07..da01e4d7 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -49,6 +49,9 @@ namespace rviz { class Property; class PropertyTreeModel; } +namespace moveit { namespace task_constructor { +class Stage; +} } namespace moveit_rviz_plugin { @@ -57,32 +60,45 @@ class PropertyFactory public: static PropertyFactory& instance(); - typedef std::function FactoryFunction; + typedef std::function PropertyFactoryFunction; + typedef std::function TreeFactoryFunction; - /// register a new factory function for type T + /// register a factory function for type T template - inline void registerType(const FactoryFunction& f) { + inline void registerType(const PropertyFactoryFunction& f) { moveit::task_constructor::PropertySerializer(); // register serializer registerType(moveit::task_constructor::Property::typeName(typeid(T)), f); } + /// register a factory function for stage T + template + inline void registerStage(const TreeFactoryFunction& f) { registerStage(typeid(T), f); } + /// create rviz::Property for given MTC Property rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property &prop) const; /// create rviz::Property for given MTC property message rviz::Property* create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const; + /// create PropertyTreeModel for given Stage + rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage &stage); + + /// turn a PropertyMap into an rviz::PropertyTreeModel + rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap &properties); + + /// add all properties from map that are not yet in root + void addRemainingProperties(rviz::Property *root, moveit::task_constructor::PropertyMap &properties); + private: - std::map registry_; + std::map property_registry_; + std::map stage_registry_; /// class is singleton PropertyFactory(); PropertyFactory(const PropertyFactory&) = delete; void operator=(const PropertyFactory&) = delete; - void registerType(const std::string& type_name, const FactoryFunction& f); + void registerType(const std::string& type_name, const PropertyFactoryFunction& f); + void registerStage(const std::type_index& type_index, const TreeFactoryFunction& f); }; -/// turn a PropertyMap into an rviz::PropertyTreeModel -rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap &properties, QObject *parent = nullptr); - } diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index cbf3a945..b80bde31 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -38,6 +38,7 @@ #include "factory_model.h" #include "properties/property_factory.h" #include +#include #include @@ -247,8 +248,10 @@ rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &ind auto it_inserted = properties_.insert(std::make_pair(n, nullptr)); // TODO: We might need custom factory methods to create PropertyTreeModels // that are specifically tailored to a Stage class. - if (it_inserted.second) // newly inserted, create new model - it_inserted.first->second = defaultPropertyTreeModel(n->me()->properties(), this); + if (it_inserted.second) { // newly inserted, create new model + it_inserted.first->second = PropertyFactory::instance().createPropertyTreeModel(*n->me()); + it_inserted.first->second->setParent(this); + } return it_inserted.first->second; } From f9715f0c1837b0aad3385be30ebd036095c228cb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Jan 2019 20:35:46 +0100 Subject: [PATCH 07/14] pass PlanningScene + DisplayContext to rviz::Property creation ... to allow properties to display scene-specific lists or create rviz markers --- .../properties/property_factory.cpp | 38 ++++++++++++------- .../properties/property_factory.h | 27 ++++++++++--- .../src/local_task_model.cpp | 18 +++------ .../src/local_task_model.h | 7 +++- .../src/task_display.cpp | 1 + .../src/task_list_model.cpp | 19 +++++++++- .../src/task_list_model.h | 20 +++++----- .../motion_planning_tasks/src/task_panel.cpp | 2 +- .../test/test_task_model.cpp | 6 ++- 9 files changed, 92 insertions(+), 46 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 52ccb933..268db464 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -48,9 +48,11 @@ namespace mtc = ::moveit::task_constructor; namespace moveit_rviz_plugin { -static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop) { +static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene*, + rviz::DisplayContext*) { std::string value; - if (mtc_prop.defined()) + if (!mtc_prop.value().empty()) value = boost::any_cast(mtc_prop.value()); rviz::StringProperty* rviz_prop = new rviz::StringProperty(name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description())); @@ -59,8 +61,10 @@ static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& m return rviz_prop; } template -static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop) { - T value = mtc_prop.defined() ? boost::any_cast(mtc_prop.value()) : T(); +static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene*, + rviz::DisplayContext*) { + T value = !mtc_prop.value().empty() ? boost::any_cast(mtc_prop.value()) : T(); rviz::FloatProperty* rviz_prop = new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); QObject::connect(rviz_prop, &rviz::FloatProperty::changed, [rviz_prop, &mtc_prop]() {mtc_prop.setValue(T(rviz_prop->getFloat()));}); @@ -93,11 +97,13 @@ void PropertyFactory::registerStage(const std::type_index &type_index, const Pro stage_registry_.insert(std::make_pair(type_index, f)); } -rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop) const +rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context) const { auto it = property_registry_.find(prop.typeName()); if (it == property_registry_.end()) return nullptr; - return it->second(QString::fromStdString(prop_name), prop); + return it->second(QString::fromStdString(prop_name), prop, scene, display_context); } rviz::Property* PropertyFactory::create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const @@ -115,17 +121,21 @@ rviz::Property* PropertyFactory::create(const moveit_task_constructor_msgs::Prop } } -rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage &stage) +rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context) { auto it = stage_registry_.find(typeid(stage)); if (it == stage_registry_.end()) - return defaultPropertyTreeModel(stage.properties()); - return it->second(stage.properties()); + return defaultPropertyTreeModel(stage.properties(), scene, display_context); + return it->second(stage.properties(), scene, display_context); } -rviz::PropertyTreeModel* PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties) { +rviz::PropertyTreeModel* PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context) { auto root = new rviz::Property(); - addRemainingProperties(root, properties); + addRemainingProperties(root, properties, scene, display_context); return new rviz::PropertyTreeModel(root, nullptr); } @@ -137,13 +147,15 @@ static bool hasChild(rviz::Property* root, const QString& name) { return false; } -void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::PropertyMap& properties) { +void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::PropertyMap& properties, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context) { for (auto& prop : properties) { const QString& name = QString::fromStdString(prop.first); if (hasChild(root, name)) continue; - rviz::Property* rviz_prop = create(prop.first, prop.second); + rviz::Property* rviz_prop = create(prop.first, prop.second, scene, display_context); if (!rviz_prop) rviz_prop = new rviz::Property(name); root->addChild(rviz_prop); } diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index da01e4d7..dff60174 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -48,6 +48,10 @@ namespace rviz { class Property; class PropertyTreeModel; +class DisplayContext; +} +namespace planning_scene { +class PlanningScene; } namespace moveit { namespace task_constructor { class Stage; @@ -60,8 +64,12 @@ class PropertyFactory public: static PropertyFactory& instance(); - typedef std::function PropertyFactoryFunction; - typedef std::function TreeFactoryFunction; + typedef std::function PropertyFactoryFunction; + typedef std::function TreeFactoryFunction; /// register a factory function for type T template @@ -75,18 +83,25 @@ public: inline void registerStage(const TreeFactoryFunction& f) { registerStage(typeid(T), f); } /// create rviz::Property for given MTC Property - rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property &prop) const; + rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property &prop, + const planning_scene::PlanningScene*scene, rviz::DisplayContext *display_context) const; /// create rviz::Property for given MTC property message rviz::Property* create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const; /// create PropertyTreeModel for given Stage - rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage &stage); + rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage &stage, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context); /// turn a PropertyMap into an rviz::PropertyTreeModel - rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap &properties); + rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap &properties, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context); /// add all properties from map that are not yet in root - void addRemainingProperties(rviz::Property *root, moveit::task_constructor::PropertyMap &properties); + void addRemainingProperties(rviz::Property *root, moveit::task_constructor::PropertyMap &properties, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context); private: std::map property_registry_; diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index b80bde31..dc829158 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -78,21 +78,17 @@ QModelIndex LocalTaskModel::index(Node *n) const return QModelIndex(); } -LocalTaskModel::LocalTaskModel(QObject *parent) +LocalTaskModel::LocalTaskModel(ContainerBase::pointer &&container, const planning_scene::PlanningSceneConstPtr& scene, + rviz::DisplayContext* display_context, QObject* parent) : BaseTaskModel(parent) - , Task() + , Task("", std::move(container)) + , scene_(scene) + , display_context_(display_context) { root_ = pimpl(); flags_ |= LOCAL_MODEL; } -LocalTaskModel::LocalTaskModel(ContainerBase::pointer &&container, QObject *parent) - : BaseTaskModel(parent) - , Task("", std::move(container)) -{ - root_ = pimpl(); -} - int LocalTaskModel::rowCount(const QModelIndex &parent) const { if (parent.column() > 0) @@ -246,10 +242,8 @@ rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &ind Node *n = node(index); if (!n) return nullptr; auto it_inserted = properties_.insert(std::make_pair(n, nullptr)); - // TODO: We might need custom factory methods to create PropertyTreeModels - // that are specifically tailored to a Stage class. if (it_inserted.second) { // newly inserted, create new model - it_inserted.first->second = PropertyFactory::instance().createPropertyTreeModel(*n->me()); + it_inserted.first->second = PropertyFactory::instance().createPropertyTreeModel(*n->me(), scene_.get(), display_context_); it_inserted.first->second->setParent(this); } return it_inserted.first->second; diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index dbedd86f..9d685bc2 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -49,14 +49,17 @@ class LocalTaskModel typedef moveit::task_constructor::StagePrivate Node; Node *root_; StageFactoryPtr stage_factory_; + planning_scene::PlanningSceneConstPtr scene_; + rviz::DisplayContext* display_context_; + std::map properties_; inline Node* node(const QModelIndex &index) const; QModelIndex index(Node *n) const; public: - LocalTaskModel(QObject *parent = nullptr); - LocalTaskModel(ContainerBase::pointer &&container, QObject *parent = nullptr); + LocalTaskModel(ContainerBase::pointer &&container, const planning_scene::PlanningSceneConstPtr &scene, + rviz::DisplayContext* display_context, QObject *parent = nullptr); int rowCount(const QModelIndex &parent = QModelIndex()) const override; QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override; diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index f94ef26f..8c0bb2a2 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -101,6 +101,7 @@ void TaskDisplay::onInitialize() { Display::onInitialize(); trajectory_visual_->onInitialize(scene_node_, context_); + task_list_model_->setDisplayContext(context_); // create a new TaskPanel by default // by post-poning this to main loop, we can ensure that rviz has loaded everything before mainloop_jobs_.addJob([this]() { TaskPanel::incDisplayCount(context_->getWindowManager()); }); diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 52a17355..57e02fa5 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -187,6 +187,11 @@ void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr &scene) scene_ = scene; } +void TaskListModel::setDisplayContext(rviz::DisplayContext *display_context) +{ + display_context_ = display_context; +} + void TaskListModel::setSolutionClient(ros::ServiceClient *client) { get_solution_client_ = client; @@ -319,6 +324,18 @@ DisplaySolutionPtr TaskListModel::processSolutionMessage(const std::string &id, return remote_task->processSolutionMessage(msg); } +bool TaskListModel::insertModel(BaseTaskModel *model, int pos) { + Q_ASSERT(model && model->columnCount() == columnCount()); + // pass on stage factory + model->setStageFactory(stage_factory_); + // forward to base class method + return FlatMergeProxyModel::insertModel(model, pos); +} + +BaseTaskModel* TaskListModel::createLocalTaskModel() { + return new LocalTaskModel(std::make_unique("task pipeline"), scene_, display_context_, this); +} + bool TaskListModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) { Q_UNUSED(column); @@ -340,7 +357,7 @@ bool TaskListModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, i } // create a new local task using the given container as root - auto *m = new LocalTaskModel(std::move(container), this); + auto *m = new LocalTaskModel(std::move(container), scene_, display_context_, this); insertModel(m, row); return true; } diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index d4aa3a27..cfa30e84 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -51,7 +51,10 @@ #include namespace ros { class ServiceClient; } -namespace rviz { class PropertyTreeModel; } +namespace rviz { +class PropertyTreeModel; +class DisplayContext; +} namespace moveit_rviz_plugin { @@ -105,7 +108,7 @@ public: * Each TaskDisplay owns a TaskListModel to maintain the list of tasks published on * a monitoring topic. * - * Local instances are created by insertLocalTask(). + * Local instances are created by createLocalTaskModel() or in dropMimeData(). * Remote instances are discovered via processTaskMessage() / processSolutionMessage(). */ class TaskListModel : public utils::FlatMergeProxyModel { @@ -113,6 +116,8 @@ class TaskListModel : public utils::FlatMergeProxyModel { // planning scene / robot model used by all tasks in this model planning_scene::PlanningSceneConstPtr scene_; + // rviz::DisplayContext used to show (interactive) markers by the property models + rviz::DisplayContext* display_context_ = nullptr; // map from remote task IDs to tasks // if task is destroyed remotely, it is marked with flag IS_DESTROYED @@ -133,6 +138,7 @@ public: ~TaskListModel(); void setScene(const planning_scene::PlanningSceneConstPtr& scene); + void setDisplayContext(rviz::DisplayContext* display_context); void setSolutionClient(ros::ServiceClient* client); void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; } @@ -149,13 +155,9 @@ public: DisplaySolutionPtr processSolutionMessage(const std::string &id, const moveit_task_constructor_msgs::Solution &msg); /// insert a TaskModel, pos is relative to modelCount() - inline bool insertModel(BaseTaskModel* model, int pos = -1) { - Q_ASSERT(model && model->columnCount() == columnCount()); - // pass on stage factory - model->setStageFactory(stage_factory_); - // forward to base class method - return FlatMergeProxyModel::insertModel(model, pos); - } + bool insertModel(BaseTaskModel* model, int pos = -1); + /// create a new LocalTaskModel + BaseTaskModel* createLocalTaskModel(); /// providing a StageFactory makes the model accepting drops void setStageFactory(const StageFactoryPtr &factory); diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index a50ba07e..b6593f25 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -330,7 +330,7 @@ void TaskView::addTask() bool is_top_level = !current.parent().isValid(); TaskListModel* task_list_model = d_ptr->getTaskListModel(current).first; - task_list_model->insertModel(new LocalTaskModel(task_list_model), is_top_level ? -1 : current.row()); + task_list_model->insertModel(task_list_model->createLocalTaskModel(), is_top_level ? -1 : current.row()); // select and edit newly inserted model if (is_top_level) current = current.model()->index(task_list_model->rowCount()-1, 0, current); diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 512577fb..30014562 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -169,11 +169,13 @@ TEST_F(TaskListModelTest, localTaskModel) { ros::init(argc, &argv, "testLocalTaskModel"); children = 3; - moveit_rviz_plugin::LocalTaskModel m; + const char* task_name = "task pipeline"; + moveit_rviz_plugin::LocalTaskModel m(std::make_unique(task_name), + planning_scene::PlanningSceneConstPtr(), nullptr); for (int i = 0; i != children; ++i) m.add(std::make_unique(std::to_string(i))); - { SCOPED_TRACE("localTaskModel"); validate(m, {"task pipeline"}); } + { SCOPED_TRACE("localTaskModel"); validate(m, {task_name}); } } TEST_F(TaskListModelTest, noChildren) { From 4ca794cb860ef9f4f766183c1e3ce097b964e763 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Jan 2019 17:56:57 +0100 Subject: [PATCH 08/14] unify property handling in LocalTaskModel and RemoteTaskModel --- .../moveit/task_constructor/properties.h | 3 + core/src/properties.cpp | 4 ++ .../properties/property_factory.cpp | 15 ----- .../properties/property_factory.h | 17 +++-- .../src/local_task_model.cpp | 4 +- .../src/local_task_model.h | 3 - .../src/remote_task_model.cpp | 63 ++++++++++++++++--- .../src/remote_task_model.h | 3 +- .../src/task_list_model.cpp | 2 +- .../src/task_list_model.h | 7 ++- .../test/test_task_model.cpp | 2 +- 11 files changed, 82 insertions(+), 41 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 83f8a8ae..e4ed0c18 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -78,6 +78,9 @@ class Property { public: typedef boost::typeindex::type_info type_info; + /// Construct a property holding a any value + Property(); + /// base class for Property exceptions class error; /// exception thrown when accessing an undeclared property diff --git a/core/src/properties.cpp b/core/src/properties.cpp index a4b34a49..a26178dc 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -127,6 +127,10 @@ Property::Property(const type_info& type_info, const std::string& description, c reset(); } +Property::Property() : Property(typeid(boost::any), "", boost::any()) +{ +} + void Property::setValue(const boost::any &value) { setCurrentValue(value); default_ = value_; diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 268db464..19ca34ca 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -106,21 +106,6 @@ rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Prope return it->second(QString::fromStdString(prop_name), prop, scene, display_context); } -rviz::Property* PropertyFactory::create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const -{ - if (old) { // reuse existing Property? - old->setDescription(QString::fromStdString(p.description)); - old->setValue(QString::fromStdString(p.value)); - return old; - } else { // create new Property? - rviz::Property *result = new rviz::StringProperty(QString::fromStdString(p.name), - QString::fromStdString(p.value), - QString::fromStdString(p.description)); - result->setReadOnly(true); - return result; - } -} - rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage, const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context) diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index dff60174..1051ad12 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -43,7 +43,6 @@ #include #include -#include namespace rviz { class Property; @@ -59,6 +58,16 @@ class Stage; namespace moveit_rviz_plugin { +/** Registry for rviz::Property and rviz::PropertyTreeModel creator functions. + * + * To inspect (and edit) properties of stages, our MTC properties are converted to rviz properties, + * which are finally shown in an rviz::PropertyTree. + * To allow customization of property display, one can register creator functions for individual + * properties as well as creator functions for a complete stage. The latter allows to fully customize + * the display of stage properties, e.g. hiding specific properties, or returning a subclassed + * PropertyTreeModel with modified behaviour. By default, defaultPropertyTreeModel() creates an rviz + * property for each MTC property. + */ class PropertyFactory { public: @@ -75,7 +84,7 @@ public: template inline void registerType(const PropertyFactoryFunction& f) { moveit::task_constructor::PropertySerializer(); // register serializer - registerType(moveit::task_constructor::Property::typeName(typeid(T)), f); + registerType(moveit::task_constructor::PropertySerializer::typeName(), f); } /// register a factory function for stage T @@ -84,9 +93,7 @@ public: /// create rviz::Property for given MTC Property rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property &prop, - const planning_scene::PlanningScene*scene, rviz::DisplayContext *display_context) const; - /// create rviz::Property for given MTC property message - rviz::Property* create(const moveit_task_constructor_msgs::Property& p, rviz::Property* old) const; + const planning_scene::PlanningScene *scene, rviz::DisplayContext *display_context) const; /// create PropertyTreeModel for given Stage rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage &stage, diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index dc829158..53e7ea83 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -80,10 +80,8 @@ QModelIndex LocalTaskModel::index(Node *n) const LocalTaskModel::LocalTaskModel(ContainerBase::pointer &&container, const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, QObject* parent) - : BaseTaskModel(parent) + : BaseTaskModel(scene, display_context, parent) , Task("", std::move(container)) - , scene_(scene) - , display_context_(display_context) { root_ = pimpl(); flags_ |= LOCAL_MODEL; diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index 9d685bc2..7d6a2742 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -49,9 +49,6 @@ class LocalTaskModel typedef moveit::task_constructor::StagePrivate Node; Node *root_; StageFactoryPtr stage_factory_; - planning_scene::PlanningSceneConstPtr scene_; - rviz::DisplayContext* display_context_; - std::map properties_; inline Node* node(const QModelIndex &index) const; diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 0c757c4c..48cb4aea 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -39,6 +39,7 @@ #include "remote_task_model.h" #include "properties/property_factory.h" #include +#include #include #include #include @@ -67,11 +68,12 @@ struct RemoteTaskModel::Node { InterfaceFlags interface_flags_; NodeFlags node_flags_; std::unique_ptr solutions_; - std::unique_ptr properties_; + std::unique_ptr property_tree_; + std::map properties_; inline Node(Node *parent) : parent_(parent) { solutions_.reset(new RemoteSolutionModel()); - properties_.reset(new rviz::PropertyTreeModel(new rviz::Property())); + property_tree_.reset(new rviz::PropertyTreeModel(new rviz::Property())); } bool setName(const QString& name) { @@ -80,13 +82,20 @@ struct RemoteTaskModel::Node { return true; } - void setProperties(const std::vector& props); + void setProperties(const std::vector& props, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz::DisplayContext* display_context_); + rviz::Property *createProperty(const moveit_task_constructor_msgs::Property &prop, rviz::Property *old, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz::DisplayContext* display_context_); }; -void RemoteTaskModel::Node::setProperties(const std::vector &props) +void RemoteTaskModel::Node::setProperties(const std::vector &props, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz::DisplayContext* display_context_) { // insert properties in same order as reported in description - rviz::Property *root = properties_->getRoot(); + rviz::Property *root = property_tree_->getRoot(); int index = 0; // current child index in root for (auto it = props.begin(); it != props.end(); ++it) { int num = root->numChildren(); @@ -103,7 +112,7 @@ void RemoteTaskModel::Node::setProperties(const std::vectorgetName().toStdString() != it->name) old_child = nullptr; - rviz::Property *new_child = PropertyFactory::instance().create(*it, old_child); + rviz::Property *new_child = createProperty(*it, old_child, scene_, display_context_); if (new_child != old_child) root->addChild(new_child, index); ++index; @@ -112,6 +121,38 @@ void RemoteTaskModel::Node::setProperties(const std::vectorremoveChildren(index, root->numChildren()-index); } +rviz::Property* +RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Property& prop, rviz::Property* old, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz::DisplayContext* display_context_) +{ + auto& factory = PropertyFactory::instance(); + // try to deserialize from msg (using registered functions) + boost::any value = Property::deserialize(prop.type, prop.value); + if (!value.empty()) { + auto it = properties_.insert(std::make_pair(prop.name, Property())).first; + it->second.setDescription(prop.description); + it->second.setValue(value); + if (rviz::Property* rviz_prop = factory.create(prop.name, it->second, scene_.get(), display_context_)) { + // rviz_prop->setReadOnly(true); + return rviz_prop; + } else + properties_.erase(it); + } + + if (old) { // reuse existing Property? + old->setDescription(QString::fromStdString(prop.description)); + old->setValue(QString::fromStdString(prop.value)); + return old; + } else { // create new Property? + rviz::Property *result = new rviz::StringProperty(QString::fromStdString(prop.name), + QString::fromStdString(prop.value), + QString::fromStdString(prop.description)); + result->setReadOnly(true); + return result; + } +} + // return Node* corresponding to index RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex &index) const { @@ -152,8 +193,10 @@ QModelIndex RemoteTaskModel::index(const Node *n) const return QModelIndex(); } -RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, QObject *parent) - : BaseTaskModel(parent), root_(new Node(nullptr)), scene_(scene) +RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, + rviz::DisplayContext* display_context, QObject *parent) + : BaseTaskModel(scene, display_context, parent) + , root_(new Node(nullptr)) { id_to_stage_[0] = root_; // root node has ID 0 } @@ -292,7 +335,7 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg if (!(n->node_flags_ & NAME_CHANGED)) // avoid overwriting a manually changed name changed |= n->setName(QString::fromStdString(s.name)); - n->setProperties(s.properties); + n->setProperties(s.properties, scene_, display_context_); InterfaceFlags old_flags = n->interface_flags_; n->interface_flags_ = InterfaceFlags(); @@ -422,7 +465,7 @@ rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex &in { Node *n = node(index); if (!n) return nullptr; - return n->properties_.get(); + return n->property_tree_.get(); } namespace detail { diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index 671aa151..80025284 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -54,7 +54,6 @@ class RemoteTaskModel : public BaseTaskModel { Q_OBJECT struct Node; Node* const root_; - planning_scene::PlanningSceneConstPtr scene_; ros::ServiceClient* get_solution_client_ = nullptr; std::map id_to_stage_; @@ -67,7 +66,7 @@ class RemoteTaskModel : public BaseTaskModel { inline RemoteSolutionModel* getSolutionModel(uint32_t stage_id) const; public: - RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, QObject *parent = nullptr); + RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, rviz::DisplayContext *display_context, QObject *parent = nullptr); ~RemoteTaskModel(); void setSolutionClient(ros::ServiceClient *client); diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 57e02fa5..8cfa1eea 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -275,7 +275,7 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id, } } else if (created) { // create new task model, if ID was not known before // the model is managed by this instance via Qt's parent-child mechanism - remote_task = new RemoteTaskModel(scene_, this); + remote_task = new RemoteTaskModel(scene_, display_context_, this); remote_task->setSolutionClient(get_solution_client_); // HACK: always use the last created model as active diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index cfa30e84..610197be 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -70,6 +70,8 @@ class BaseTaskModel : public QAbstractItemModel { Q_OBJECT protected: unsigned int flags_ = 0; + planning_scene::PlanningSceneConstPtr scene_; + rviz::DisplayContext* display_context_; public: enum TaskModelFlag { @@ -79,7 +81,10 @@ public: IS_RUNNING = 0x08, }; - BaseTaskModel(QObject *parent = nullptr) : QAbstractItemModel(parent) {} + BaseTaskModel(const planning_scene::PlanningSceneConstPtr &scene, + rviz::DisplayContext* display_context, QObject *parent = nullptr) + : QAbstractItemModel(parent), scene_(scene), display_context_(display_context) + {} int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; } QVariant headerData(int section, Qt::Orientation orientation, int role) const override; diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 30014562..0e32372b 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -157,7 +157,7 @@ protected: TEST_F(TaskListModelTest, remoteTaskModel) { children = 3; planning_scene::PlanningSceneConstPtr scene; - moveit_rviz_plugin::RemoteTaskModel m(scene); + moveit_rviz_plugin::RemoteTaskModel m(scene, nullptr); m.processStageDescriptions(genMsg("first").stages); SCOPED_TRACE("first"); validate(m, {"first"}); From a1c81f12366c37e342461b7af40e74a893445b16 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 7 Feb 2019 20:44:19 +0100 Subject: [PATCH 09/14] YAML property serialization - switch from ROS serialization/deserialization to YAML - no native C++ deserialization for ROS msg types available - drop Propert::print() --- .../moveit/task_constructor/properties.h | 49 +++---------------- core/src/properties.cpp | 25 +++------- 2 files changed, 13 insertions(+), 61 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index e4ed0c18..7d911054 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -109,11 +109,9 @@ public: /// get default value const boost::any& defaultValue() const { return default_; } - /// serialize/print value using registered functions - static std::string print(const boost::any& value); + /// serialize value using registered functions static std::string serialize(const boost::any& value); static boost::any deserialize(const std::string& type_name, const std::string& wire); - std::string print() const { return print(value()); } std::string serialize() const { return serialize(value()); } /// get description text @@ -186,14 +184,13 @@ class PropertySerializerBase { public: typedef std::string (*SerializeFunction)(const boost::any&); typedef boost::any (*DeserializeFunction)(const std::string&); - typedef std::string (*PrintFunction)(const boost::any&); static std::string dummySerialize(const boost::any&) { return ""; } static boost::any dummyDeserialize(const std::string&) { return boost::any(); } protected: static bool insert(const std::type_index& type_index, const std::string& type_name, - SerializeFunction serialize, DeserializeFunction deserialize, PrintFunction print); + SerializeFunction serialize, DeserializeFunction deserialize); }; /// utility class to register serializer/deserializer functions for a property of type T @@ -201,7 +198,7 @@ template class PropertySerializer : protected PropertySerializerBase { public: PropertySerializer() { - insert(typeid(T), typeName(), &serialize, &deserialize, &print); + insert(typeid(T), typeName(), &serialize, &deserialize); } template @@ -215,46 +212,16 @@ public: typeName() { return typeid(T).name(); } private: - /** ROS message serialization */ - template - static typename std::enable_if::value, std::string>::type - serialize(const boost::any& value) { - static_assert(sizeof(uint8_t) == sizeof(char), "Assuming char has same size as uint8_t"); - const T& msg = boost::any_cast(value); - std::size_t size = ros::serialization::serializationLength(msg); - std::string result(size, '\0'); - if (size) - { - ros::serialization::OStream stream(reinterpret_cast(&result[0]), size); - ros::serialization::serialize(stream, msg); - } - return result; - } - template - static typename std::enable_if::value, boost::any>::type - deserialize(const std::string& wire) { - T value; - ros::serialization::IStream stream(const_cast(reinterpret_cast(&wire[0])), wire.size()); - ros::serialization::deserialize(stream, value); - return value; - } - /** Serialization based on std::[io]stringstream */ template static typename std::enable_if::value, std::string>::type - print(const boost::any& value) { + serialize(const boost::any& value) { std::ostringstream oss; oss << boost::any_cast(value); return oss.str(); } - template - static typename std::enable_if::value && hasSerialize::value, std::string>::type - serialize(const boost::any& value) { - return print(value); - } - template - static typename std::enable_if::value && hasSerialize::value && hasDeserialize::value, boost::any>::type + static typename std::enable_if::value && hasDeserialize::value, boost::any>::type deserialize(const std::string& wired) { std::istringstream iss(wired); T value; @@ -265,13 +232,9 @@ private: /** No serialization available */ template static typename std::enable_if::value, std::string>::type - print(const boost::any& value) { return dummySerialize(value); } - - template - static typename std::enable_if::value && !hasSerialize::value, std::string>::type serialize(const boost::any& value) { return dummySerialize(value); } template - static typename std::enable_if::value && (!hasSerialize::value || !hasDeserialize::value), boost::any>::type + static typename std::enable_if::value || !hasDeserialize::value, boost::any>::type deserialize(const std::string& wire) { return dummyDeserialize(wire); } }; diff --git a/core/src/properties.cpp b/core/src/properties.cpp index a26178dc..393fdcf5 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -51,7 +51,6 @@ class PropertyTypeRegistry { std::string name_; PropertySerializerBase::SerializeFunction serialize_; PropertySerializerBase::DeserializeFunction deserialize_; - PropertySerializerBase::PrintFunction print_; }; Entry dummy_; @@ -64,13 +63,11 @@ class PropertyTypeRegistry { public: PropertyTypeRegistry() : dummy_{"", PropertySerializerBase::dummySerialize, - PropertySerializerBase::dummyDeserialize, - PropertySerializerBase::dummySerialize} + PropertySerializerBase::dummyDeserialize} {} inline bool insert(const std::type_index& type_index, const std::string& type_name, PropertySerializerBase::SerializeFunction serialize, - PropertySerializerBase::DeserializeFunction deserialize, - PropertySerializerBase::PrintFunction print); + PropertySerializerBase::DeserializeFunction deserialize); const Entry& entry(const std::type_index& type_index) const { auto it = types_.find(type_index); @@ -91,13 +88,12 @@ static PropertyTypeRegistry registry_singleton_; bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std::string& type_name, PropertySerializerBase::SerializeFunction serialize, - PropertySerializerBase::DeserializeFunction deserialize, - PropertySerializerBase::PrintFunction print) + PropertySerializerBase::DeserializeFunction deserialize) { if (type_index == std::type_index(typeid(boost::any))) return false; - auto it_inserted = types_.insert(std::make_pair(type_index, Entry {type_name, serialize, deserialize, print})); + auto it_inserted = types_.insert(std::make_pair(type_index, Entry {type_name, serialize, deserialize})); if (!it_inserted.second) return false; // was already registered before @@ -109,9 +105,9 @@ bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std:: bool PropertySerializerBase::insert(const std::type_index& type_index, const std::string& type_name, PropertySerializerBase::SerializeFunction serialize, - PropertySerializerBase::DeserializeFunction deserialize, PrintFunction print) + PropertySerializerBase::DeserializeFunction deserialize) { - return registry_singleton_.insert(type_index, type_name, serialize, deserialize, print); + return registry_singleton_.insert(type_index, type_name, serialize, deserialize); } @@ -169,13 +165,6 @@ boost::any Property::deserialize(const std::string& type_name, const std::string return registry_singleton_.entry(type_name).deserialize_(wire); } -std::string Property::print(const boost::any& value) -{ - if (value.empty()) - return ""; - return registry_singleton_.entry(value.type()).print_(value); -} - std::string Property::typeName() const { if (value().empty()) return typeName(type_info_); else return typeName(value().type()); @@ -320,7 +309,7 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << - " -> " << source << ": " << Property::print(value)); + " -> " << source << ": " << Property::serialize(value)); p.setCurrentValue(value); p.initialized_from_ = source; } From 20e951bf636d6d66c26de2a2503a905995efce6f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 7 Feb 2019 22:04:41 +0100 Subject: [PATCH 10/14] PropertyFactory::createDefault() ... creating a read-only rviz::Property from serialized mtc::Property value --- .../properties/property_factory.cpp | 21 ++++++++++++++++++- .../properties/property_factory.h | 4 ++++ .../src/remote_task_model.cpp | 15 +++---------- 3 files changed, 27 insertions(+), 13 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 19ca34ca..375d0df8 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -102,7 +102,8 @@ rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Prope rviz::DisplayContext* display_context) const { auto it = property_registry_.find(prop.typeName()); - if (it == property_registry_.end()) return nullptr; + if (it == property_registry_.end()) + return createDefault(prop_name, prop.typeName(), prop.description(), prop.serialize()); return it->second(QString::fromStdString(prop_name), prop, scene, display_context); } @@ -150,4 +151,22 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property new rviz::Property("no properties", QVariant(), QString(), root); } +rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, + const std::string& description, const std::string& value, + rviz::Property* old) +{ + if (old) { // reuse existing Property? + assert(old->getNameStd() == name); + old->setDescription(QString::fromStdString(description)); + old->setValue(QString::fromStdString(value)); + return old; + } else { // create new Property? + rviz::Property *result = new rviz::StringProperty(QString::fromStdString(name), + QString::fromStdString(value), + QString::fromStdString(description)); + result->setReadOnly(true); + return result; + } +} + } diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index 1051ad12..3c7c1d33 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -94,6 +94,10 @@ public: /// create rviz::Property for given MTC Property rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property &prop, const planning_scene::PlanningScene *scene, rviz::DisplayContext *display_context) const; + /// create rviz::Property for property of given name, type, description, and value + static rviz::Property* createDefault(const std::string& name, const std::string& type, + const std::string& description, const std::string& value, + rviz::Property* old=nullptr); /// create PropertyTreeModel for given Stage rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage &stage, diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 48cb4aea..c2bb3d49 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -129,7 +129,7 @@ RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Proper auto& factory = PropertyFactory::instance(); // try to deserialize from msg (using registered functions) boost::any value = Property::deserialize(prop.type, prop.value); - if (!value.empty()) { + if (!value.empty()) { // if successful, create rviz::Property from mtc::Property using factory methods auto it = properties_.insert(std::make_pair(prop.name, Property())).first; it->second.setDescription(prop.description); it->second.setValue(value); @@ -140,17 +140,8 @@ RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Proper properties_.erase(it); } - if (old) { // reuse existing Property? - old->setDescription(QString::fromStdString(prop.description)); - old->setValue(QString::fromStdString(prop.value)); - return old; - } else { // create new Property? - rviz::Property *result = new rviz::StringProperty(QString::fromStdString(prop.name), - QString::fromStdString(prop.value), - QString::fromStdString(prop.description)); - result->setReadOnly(true); - return result; - } + // otherwise create default, read-only rviz::Property by parsing serialized YAML + return factory.createDefault(prop.name, prop.type, prop.description, prop.value, old); } // return Node* corresponding to index From e71376743cbfa173a84b71ce8f98d5d319edaa07 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 8 Feb 2019 17:24:17 +0100 Subject: [PATCH 11/14] 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); + } +} + +} From 9810e3bb4b57a77dbc034ada7c05ba78263e9e3a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 9 Feb 2019 16:12:17 +0100 Subject: [PATCH 12/14] wip: understand event-based, incremental parsing with libyaml --- .../properties/CMakeLists.txt | 2 +- .../properties/property_from_yaml.cpp | 46 +++++++++++++++++-- 2 files changed, 43 insertions(+), 5 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index 9632287d..8742141e 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -5,7 +5,7 @@ set(SOURCES ) include(FindPkgConfig) -pkg_check_modules(YAML yaml-cpp>=0.5) +pkg_check_modules(YAML yaml-0.1) 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}) diff --git a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp index ecf3fdbe..ce7b581c 100644 --- a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -35,7 +35,7 @@ /* Author: Robert Haschke */ #include "property_factory.h" -#include +#include #include #include @@ -48,7 +48,7 @@ namespace moveit_rviz_plugin { * 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 // 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) { @@ -183,6 +183,9 @@ rviz::Property* createFromNode(const QString& name, const QString& description, result->setReadOnly(true); return result; } +#endif + +inline std::string indent(unsigned int depth) { return std::string(2*depth, ' '); } rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, const std::string& description, const std::string& value, @@ -190,10 +193,45 @@ 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 { - return createFromNode(qname, qdesc, YAML::Load(value), old); + 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) { - return createFromNode(qname, qdesc, dummyNode("YAML parse error"), old); + std::cout << e.what() << std::endl; + return nullptr; } } From bb61513cb81ffb2a3e9922917b2256a38a5841f4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 9 Feb 2019 20:26:35 +0100 Subject: [PATCH 13/14] 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); } } From e815fb41c2d0f8b66c71fa0ceefb8f3f8913be84 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 9 Feb 2019 20:30:12 +0100 Subject: [PATCH 14/14] RemoteTaskModel: all props read-only ... including ones registered in PropertyFactory --- visualization/motion_planning_tasks/src/remote_task_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index c2bb3d49..cb15c6c3 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -134,7 +134,7 @@ RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Proper it->second.setDescription(prop.description); it->second.setValue(value); if (rviz::Property* rviz_prop = factory.create(prop.name, it->second, scene_.get(), display_context_)) { - // rviz_prop->setReadOnly(true); + rviz_prop->setReadOnly(true); return rviz_prop; } else properties_.erase(it);