diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 22112045..7d911054 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; @@ -77,28 +70,17 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name); 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 boost::typeindex::type_info& type_info, + const std::string &description, + const boost::any &default_value); 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 @@ -111,8 +93,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 +108,10 @@ 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 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 serialize() const { return serialize(value()); } /// get description text @@ -137,7 +119,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 type_info& type_info); + std::string typeName() const; /// return true, if property initialized from given SourceId bool initsFrom(SourceFlags source) const; @@ -146,13 +129,9 @@ 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_; + const type_info& type_info_; boost::any default_; boost::any value_; @@ -160,8 +139,6 @@ private: SourceFlags source_flags_ = 0; SourceFlags initialized_from_; InitializerFunction initializer_; - SignalFunction signaller_; - SerializeFunction serialize_; }; @@ -189,6 +166,79 @@ 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&); + + 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); +}; + +/// 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); + } + + 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: + /** Serialization based on std::[io]stringstream */ + template + static typename std::enable_if::value, std::string>::type + serialize(const boost::any& value) { + std::ostringstream oss; + oss << boost::any_cast(value); + return oss.str(); + } + template + static typename std::enable_if::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 + serialize(const boost::any& value) { return dummySerialize(value); } + template + static typename std::enable_if::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 @@ -201,21 +251,21 @@ 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, - const std::string& description, - const boost::any& default_value, - const Property::SerializeFunction &serialize); + 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 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); + const std::string& description = "") { + 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..a5297fe5 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -44,20 +44,89 @@ 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_; + }; + 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} + {} + inline bool insert(const std::type_index& type_index, const std::string& type_name, + PropertySerializerBase::SerializeFunction serialize, + PropertySerializerBase::DeserializeFunction deserialize); + + 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()); + 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) +{ + 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})); + 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) +{ + return registry_singleton_.insert(type_index, type_name, serialize, deserialize); +} + + +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) - , serialize_(serialize) { // 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(); } +Property::Property() : Property(typeid(boost::any), "", boost::any()) +{ +} + void Property::setValue(const boost::any &value) { setCurrentValue(value); default_ = value_; @@ -66,14 +135,11 @@ 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 - - if (signaller_) - signaller_(this); } void Property::reset() @@ -84,9 +150,32 @@ 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 (type_name != Property::typeName(typeid(std::string)) && wire.empty()) + return boost::any(); + else + return registry_singleton_.entry(type_name).deserialize_(wire); +} + +std::string Property::typeName() const { + if (value().empty()) return typeName(type_info_); + else return typeName(value().type()); +} + +std::string Property::typeName(const type_info& type_info) +{ + if (type_info == typeid(boost::any)) + return ""; + else + return registry_singleton_.entry(type_info).name_; } bool Property::initsFrom(Property::SourceFlags source) const @@ -110,15 +199,14 @@ 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) +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, serialize))); + 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; } @@ -145,7 +233,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_info_, p.description_, p.default_); } void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set &properties) @@ -167,8 +255,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 +308,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::serialize(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 diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index 266e15ac..8742141e 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-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}) + 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 ed5bc290..bb124b47 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -35,32 +35,48 @@ /* Author: Robert Haschke */ #include "property_factory.h" + #include +#include #include #include #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, + const planning_scene::PlanningScene*, + rviz::DisplayContext*) { + std::string value; + 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())); + 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, + 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()));}); + 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() @@ -69,32 +85,90 @@ 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) { - registry_.insert(std::make_pair(type_name, f)); + if (type_name.empty()) + return; + property_registry_.insert(std::make_pair(type_name, f)); } -rviz::Property* PropertyFactory::create(const std::string& prop_name, Property* prop) const +void PropertyFactory::registerStage(const std::type_index &type_index, const PropertyFactory::TreeFactoryFunction &f) { - auto it = registry_.find(prop->typeName()); - if (it == registry_.end()) return nullptr; - return it->second(QString::fromStdString(prop_name), prop); + stage_registry_.insert(std::make_pair(type_index, f)); } -rviz::PropertyTreeModel* createPropertyTreeModel(PropertyMap& properties, QObject* parent) { - PropertyFactory& factory = PropertyFactory::instance(); +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 createDefault(prop_name, prop.typeName(), prop.description(), prop.serialize()); + return it->second(QString::fromStdString(prop_name), prop, scene, display_context); +} - 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::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(), scene, display_context); + return it->second(stage.properties(), scene, display_context); +} + +rviz::PropertyTreeModel* PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context) { + auto root = new rviz::Property(); + addRemainingProperties(root, properties, scene, display_context); + 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; } - // just to see something, when no properties are defined - if (model->rowCount() == 0) - new rviz::Property("no properties", QVariant(), QString(), root); - return model; + return false; } +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, scene, display_context); + if (!rviz_prop) rviz_prop = new rviz::Property(name); + root->addChild(rviz_prop); + } + + // just to see something, when no properties are defined + if (root->numChildren() == 0) + 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) +{ + 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; + } +} +#endif + } diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index 6176445b..3c7c1d33 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -42,43 +42,89 @@ #include #include +#include + namespace rviz { class Property; class PropertyTreeModel; +class DisplayContext; +} +namespace planning_scene { +class PlanningScene; } namespace moveit { namespace task_constructor { -class Property; -class PropertyMap; +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: 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 - void registerType(const FactoryFunction& f) { registerType(std::type_index(typeid(T)).name(), f); } + inline void registerType(const PropertyFactoryFunction& f) { + moveit::task_constructor::PropertySerializer(); // register serializer + registerType(moveit::task_constructor::PropertySerializer::typeName(), f); + } - /// retrieve rviz property for given task_constructor property - rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property *prop) const; + /// 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 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, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context); + + /// turn a PropertyMap into an rviz::PropertyTreeModel + 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, + const planning_scene::PlanningScene* scene, + rviz::DisplayContext* display_context); 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* createPropertyTreeModel(moveit::task_constructor::PropertyMap &properties, QObject *parent = nullptr); - } 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..a64b7741 --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -0,0 +1,306 @@ +/********************************************************************* + * 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; + +/** 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). + */ + +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. +bool Parser::setValue(rviz::Property* old, const QByteArray& value) +{ + if (rviz::FloatProperty* p = dynamic_cast(old)) { + 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(value); + return true; + } + return false; +} + +// Update existing old rviz:Property or create a new one from scalar YAML node +rviz::Property *Parser::createScalar(const QString &name, const QString &description, + const QByteArray &value, rviz::Property *old) +{ + // 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; + } + + 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); + + old->setReadOnly(true); + return old; +} + +// Reuse old property (or create new one) as parent for a sequence or map +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, QVariant(), description); + old->setReadOnly(true); + } else { + old->setName(name); + old->setDescription(description); + } + return old; +} + +// Hierarchically create property from YAML map node +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 + 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; + + 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(); + + // 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); + return root; +} + +// Hierarchically create property from YAML sequence node. Items are named [#]. +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 + 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; +} + +} + +namespace moveit_rviz_plugin { + +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); + Parser parser(value); + return parser.process(qname, qdesc, old); +} + +} 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/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index a7a758a4..53e7ea83 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 @@ -77,19 +78,13 @@ QModelIndex LocalTaskModel::index(Node *n) const return QModelIndex(); } -LocalTaskModel::LocalTaskModel(QObject *parent) - : BaseTaskModel(parent) - , Task() -{ - root_ = pimpl(); - flags_ |= LOCAL_MODEL; -} - -LocalTaskModel::LocalTaskModel(ContainerBase::pointer &&container, QObject *parent) - : BaseTaskModel(parent) +LocalTaskModel::LocalTaskModel(ContainerBase::pointer &&container, const planning_scene::PlanningSceneConstPtr& scene, + rviz::DisplayContext* display_context, QObject* parent) + : BaseTaskModel(scene, display_context, parent) , Task("", std::move(container)) { root_ = pimpl(); + flags_ |= LOCAL_MODEL; } int LocalTaskModel::rowCount(const QModelIndex &parent) const @@ -245,8 +240,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)); - if (it_inserted.second) // newly inserted, create new model - it_inserted.first->second = createPropertyTreeModel(n->me()->properties(), this); + if (it_inserted.second) { // newly inserted, create new model + 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..7d6a2742 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -55,8 +55,8 @@ class LocalTaskModel 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/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 ); diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 0cbd04bf..cb15c6c3 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -37,11 +37,13 @@ #include #include "remote_task_model.h" +#include "properties/property_factory.h" #include +#include #include #include #include -#include +#include #include #include @@ -66,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) { @@ -78,8 +81,69 @@ struct RemoteTaskModel::Node { name_ = name; return true; } + + 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, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz::DisplayContext* display_context_) +{ + // insert properties in same order as reported in description + 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(); + // 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 = createProperty(*it, old_child, scene_, display_context_); + if (new_child != old_child) + root->addChild(new_child, index); + ++index; + } + // remove remaining children + root->removeChildren(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()) { // 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); + 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); + } + + // 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 RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex &index) const { @@ -120,8 +184,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 } @@ -260,6 +326,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, scene_, display_context_); + InterfaceFlags old_flags = n->interface_flags_; n->interface_flags_ = InterfaceFlags(); for (auto f : {READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END}) { @@ -388,7 +456,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_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..8cfa1eea 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; @@ -270,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 @@ -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..610197be 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 { @@ -67,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 { @@ -76,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; @@ -105,7 +113,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 +121,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 +143,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 +160,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..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"}); @@ -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) {