From 050a4c3f861e1b4b39b3abe010d198cc007ac286 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 6 Feb 2018 23:12:52 +0100 Subject: [PATCH] basic framework to display stage properties --- .../motion_planning_tasks/CMakeLists.txt | 1 + .../properties/CMakeLists.txt | 17 ++ .../properties/property_factory.cpp | 100 ++++++++++++ .../properties/property_factory.h | 84 ++++++++++ .../motion_planning_tasks/src/CMakeLists.txt | 2 +- .../src/local_task_model.cpp | 11 ++ .../src/local_task_model.h | 3 + .../src/remote_task_model.cpp | 11 ++ .../src/remote_task_model.h | 2 + .../src/task_list_model.h | 6 + .../motion_planning_tasks/src/task_panel.cpp | 10 +- .../motion_planning_tasks/src/task_view.ui | 151 +++++++++--------- 12 files changed, 324 insertions(+), 74 deletions(-) create mode 100644 visualization/motion_planning_tasks/properties/CMakeLists.txt create mode 100644 visualization/motion_planning_tasks/properties/property_factory.cpp create mode 100644 visualization/motion_planning_tasks/properties/property_factory.h diff --git a/visualization/motion_planning_tasks/CMakeLists.txt b/visualization/motion_planning_tasks/CMakeLists.txt index b64a811b..633ca748 100644 --- a/visualization/motion_planning_tasks/CMakeLists.txt +++ b/visualization/motion_planning_tasks/CMakeLists.txt @@ -1,3 +1,4 @@ add_subdirectory(utils) +add_subdirectory(properties) add_subdirectory(src) add_subdirectory(test) diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt new file mode 100644 index 00000000..4986363c --- /dev/null +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -0,0 +1,17 @@ +set(MOVEIT_LIB_NAME motion_planning_tasks_properties) + +set(SOURCES + property_factory.cpp +) +add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) + +target_link_libraries(${MOVEIT_LIB_NAME} + ${QT_LIBRARIES} +) +target_include_directories(${MOVEIT_LIB_NAME} + PUBLIC $ +) + +install(TARGETS ${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp new file mode 100644 index 00000000..ed5bc290 --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -0,0 +1,100 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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 +#include +#include + +using namespace 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())); +} + +PropertyFactory::PropertyFactory() +{ + // registe some standard types + registerType(&helper); + registerType(&helper); +} + +PropertyFactory& PropertyFactory::instance() +{ + static PropertyFactory instance_; + return instance_; +} + +void PropertyFactory::registerType(const std::string &type_name, const FactoryFunction &f) +{ + registry_.insert(std::make_pair(type_name, f)); +} + +rviz::Property* PropertyFactory::create(const std::string& prop_name, Property* prop) const +{ + auto it = registry_.find(prop->typeName()); + if (it == registry_.end()) return nullptr; + return it->second(QString::fromStdString(prop_name), prop); +} + +rviz::PropertyTreeModel* createPropertyTreeModel(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); + if (!rviz_prop) rviz_prop = new rviz::Property(QString::fromStdString(prop.first)); + rviz_prop->setParent(root); + } + // just to see something, when no properties are defined + if (model->rowCount() == 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 new file mode 100644 index 00000000..6176445b --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace rviz { +class Property; +class PropertyTreeModel; +} +namespace moveit { namespace task_constructor { +class Property; +class PropertyMap; +} } + +namespace moveit_rviz_plugin { + +class PropertyFactory +{ +public: + static PropertyFactory& instance(); + + typedef std::function FactoryFunction; + + /// register a new factory function for type T + template + void registerType(const FactoryFunction& f) { registerType(std::type_index(typeid(T)).name(), f); } + + /// retrieve rviz property for given task_constructor property + rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property *prop) const; + +private: + std::map registry_; + + /// class is singleton + PropertyFactory(); + PropertyFactory(const PropertyFactory&) = delete; + void operator=(const PropertyFactory&) = delete; + + void registerType(const std::string& type_name, const FactoryFunction& 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/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index e0032d77..c64d6114 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -26,7 +26,7 @@ add_library(${MOVEIT_LIB_NAME} set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} - motion_planning_tasks_utils moveit_task_visualization_tools + motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools ${catkin_LIBRARIES} ${QT_LIBRARIES} ) target_include_directories(${MOVEIT_LIB_NAME} diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index a5821631..b756d1c6 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -36,6 +36,7 @@ #include "local_task_model.h" #include "factory_model.h" +#include "properties/property_factory.h" #include #include @@ -232,4 +233,14 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex &index) return DisplaySolutionPtr(); } +rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &index) +{ + 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); + 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 9e9290c3..b9873a80 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -49,6 +49,7 @@ class LocalTaskModel typedef moveit::task_constructor::StagePrivate Node; Node *root_; StageFactoryPtr stage_factory_; + std::map properties_; inline Node* node(const QModelIndex &index) const; QModelIndex index(Node *n) const; @@ -73,6 +74,8 @@ public: QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override; + + rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 0b200282..59a14801 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include #include @@ -64,9 +66,11 @@ struct RemoteTaskModel::Node { InterfaceFlags interface_flags_; NodeFlags node_flags_; std::unique_ptr solutions_; + std::unique_ptr properties_; inline Node(Node *parent) : parent_(parent) { solutions_.reset(new RemoteSolutionModel()); + properties_.reset(new rviz::PropertyTreeModel(new rviz::Property())); } bool setName(const QString& name) { @@ -369,6 +373,13 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index) return it->second; } +rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex &index) +{ + Node *n = node(index); + if (!n) return nullptr; + return n->properties_.get(); +} + namespace detail { // method used by sorted_ container (requiring an additional dereference to access id) template diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index aa5ed3df..dd770cec 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -87,6 +87,8 @@ public: QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override; + + rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index b6b6f6f4..29950221 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -50,6 +50,7 @@ #include namespace ros { class ServiceClient; } +namespace rviz { class PropertyTreeModel; } namespace moveit_rviz_plugin { @@ -84,8 +85,13 @@ public: Qt::ItemFlags flags(const QModelIndex &index) const override; unsigned int taskFlags() const { return flags_; } + /// get solution model for given stage index virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0; + /// get solution for given solution index virtual DisplaySolutionPtr getSolution(const QModelIndex &index) = 0; + + /// get property model for given stage index + virtual rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0; }; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 5a2e6a3b..ae6072c6 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -254,7 +254,8 @@ void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelInd d_ptr->lock(nullptr); // unlocks any locked_display_ - auto *view = d_ptr->solutions_view; + // update the SolutionModel + QTreeView *view = d_ptr->solutions_view; QItemSelectionModel *sm = view->selectionModel(); QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr; view->sortByColumn(-1); @@ -268,6 +269,13 @@ void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelInd connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)), this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection))); } + + // update the PropertyModel + view = d_ptr->property_view; + sm = view->selectionModel(); + m = task ? task->getPropertyModel(task_index) : nullptr; + view->setModel(m); + delete sm; // we don't store the selection model } void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) diff --git a/visualization/motion_planning_tasks/src/task_view.ui b/visualization/motion_planning_tasks/src/task_view.ui index fe25c1d4..8ae8aa59 100644 --- a/visualization/motion_planning_tasks/src/task_view.ui +++ b/visualization/motion_planning_tasks/src/task_view.ui @@ -14,9 +14,6 @@ Tasks Panel - - 0 - 0 @@ -30,26 +27,14 @@ 0 - - - 0 + + + Qt::Vertical - - 0 - - - 0 - - - 0 - - - 0 - - - + + - 6 + 0 @@ -58,59 +43,76 @@ + + + + + 0 + 0 + + + + Qt::Horizontal + + + + + 2 + 0 + + + + Qt::ActionsContextMenu + + + false + + + + + + 1 + 0 + + + + QAbstractItemView::ExtendedSelection + + + false + + + true + + + true + + + false + + + + - - - - - - 0 - 0 - + + + + + 0 - - Qt::Horizontal - - - - - 2 - 0 - - - - Qt::ActionsContextMenu - - - false - - - - - - 1 - 0 - - - - QAbstractItemView::ExtendedSelection - - - false - - - true - - - true - - - false - - - - - + + + + Properties + + + + + + + + + @@ -144,6 +146,11 @@ QTreeView
task_list_model.h
+ + rviz::PropertyTreeWidget + QTreeView +
rviz/properties/property_tree_widget.h
+