basic framework to display stage properties

This commit is contained in:
Robert Haschke 2018-02-06 23:12:52 +01:00
parent 52904f65a2
commit 050a4c3f86
12 changed files with 324 additions and 74 deletions

View File

@ -1,3 +1,4 @@
add_subdirectory(utils)
add_subdirectory(properties)
add_subdirectory(src)
add_subdirectory(test)

View File

@ -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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
)
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

View File

@ -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 <boost/functional/factory.hpp>
#include <moveit/task_constructor/properties.h>
#include <rviz/properties/property_tree_model.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/float_property.h>
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 <typename T, typename RVIZProp>
RVIZProp* helper(const QString& name, Property* prop) {
T value = prop->defined() ? boost::any_cast<T>(prop->value()) : T();
return new RVIZProp(name, value, QString::fromStdString(prop->description()));
}
PropertyFactory::PropertyFactory()
{
// registe some standard types
registerType<double>(&helper<double, rviz::FloatProperty>);
registerType<QString>(&helper<QString, rviz::StringProperty>);
}
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;
}
}

View File

@ -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 <QObject>
#include <QString>
#include <map>
#include <functional>
#include <typeindex>
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<rviz::Property*(const QString& name, moveit::task_constructor::Property*)> FactoryFunction;
/// register a new factory function for type T
template <typename T>
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<std::string, FactoryFunction> 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);
}

View File

@ -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}

View File

@ -36,6 +36,7 @@
#include "local_task_model.h"
#include "factory_model.h"
#include "properties/property_factory.h"
#include <moveit/task_constructor/container_p.h>
#include <ros/console.h>
@ -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;
}
}

View File

@ -49,6 +49,7 @@ class LocalTaskModel
typedef moveit::task_constructor::StagePrivate Node;
Node *root_;
StageFactoryPtr stage_factory_;
std::map<Node*, rviz::PropertyTreeModel*> 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;
};
}

View File

@ -40,6 +40,8 @@
#include <moveit/task_constructor/container.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_task_constructor_msgs/GetSolution.h>
#include <rviz/properties/property_tree_model.h>
#include <rviz/properties/property.h>
#include <ros/console.h>
#include <ros/service_client.h>
@ -64,9 +66,11 @@ struct RemoteTaskModel::Node {
InterfaceFlags interface_flags_;
NodeFlags node_flags_;
std::unique_ptr<RemoteSolutionModel> solutions_;
std::unique_ptr<rviz::PropertyTreeModel> 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 <class T>

View File

@ -87,6 +87,8 @@ public:
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
};

View File

@ -50,6 +50,7 @@
#include <memory>
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;
};

View File

@ -254,7 +254,8 @@ void TaskView::onCurrentStageChanged(const QModelIndex &current, 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 &current, 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 &current, const QModelIndex &previous)

View File

@ -14,9 +14,6 @@
<string>Tasks Panel</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
@ -30,27 +27,15 @@
<number>0</number>
</property>
<item>
<layout class="QVBoxLayout" name="tasks_overview">
<widget class="QSplitter" name="tasks_property_splitter">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<widget class="QWidget" name="">
<layout class="QVBoxLayout" name="verticalLayout1">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<layout class="QHBoxLayout" name="tasks_view_label_layout">
<property name="spacing">
<number>6</number>
</property>
<item>
<widget class="QLabel" name="tasks_view_label">
<property name="text">
@ -58,10 +43,8 @@
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QSplitter" name="splitter">
<widget class="QSplitter" name="tasks_solutions_splitter">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
@ -111,6 +94,25 @@
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="">
<layout class="QVBoxLayout" name="verticalLayout2">
<property name="spacing">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="property_view_label">
<property name="text">
<string>Properties</string>
</property>
</widget>
</item>
<item>
<widget class="rviz::PropertyTreeWidget" name="property_view"/>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
<action name="actionRemoveTaskTreeRows">
@ -144,6 +146,11 @@
<extends>QTreeView</extends>
<header>task_list_model.h</header>
</customwidget>
<customwidget>
<class>rviz::PropertyTreeWidget</class>
<extends>QTreeView</extends>
<header location="global">rviz/properties/property_tree_widget.h</header>
</customwidget>
</customwidgets>
<resources/>
<connections/>