RemoteTaskModel: show stage properties

This commit is contained in:
Robert Haschke 2018-10-30 12:39:53 +01:00
parent 6d81743a0b
commit 5b8d841094
3 changed files with 55 additions and 3 deletions

View File

@ -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();

View File

@ -41,6 +41,7 @@
#include <map>
#include <functional>
#include <typeindex>
#include <moveit_task_constructor_msgs/Property.h>
namespace rviz {
class Property;
@ -62,10 +63,12 @@ public:
/// register a new factory function for type T
template <typename T>
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<std::string, FactoryFunction> registry_;

View File

@ -37,11 +37,12 @@
#include <stdio.h>
#include "remote_task_model.h"
#include "properties/property_factory.h"
#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 <rviz/properties/string_property.h>
#include <ros/console.h>
#include <ros/service_client.h>
@ -78,8 +79,39 @@ struct RemoteTaskModel::Node {
name_ = name;
return true;
}
void setProperties(const std::vector<moveit_task_constructor_msgs::Property>& props);
};
void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_constructor_msgs::Property> &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}) {