Merge PR #72: rviz property visualization

This commit is contained in:
Robert Haschke 2019-02-10 06:25:25 +01:00
commit 1e0a9401e7
22 changed files with 847 additions and 160 deletions

View File

@ -45,18 +45,11 @@
#include <vector>
#include <functional>
#include <sstream>
#include <ros/serialization.h>
namespace moveit {
namespace task_constructor {
// hasSerialize<T>::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 <typename T, typename = std::ostream&>
struct hasSerialize : std::false_type {};
template <typename T>
struct hasSerialize<T, decltype(std::declval<std::ostringstream&>() << std::declval<T>())> : 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<boost::any>().type()) type_index;
typedef std::function<std::string(const boost::any& v)> SerializeFunction;
Property(const type_index &type_index, const std::string &description, const boost::any &default_value,
const Property::SerializeFunction &serialize);
template <typename T>
static typename std::enable_if<hasSerialize<T>::value, std::string>::type
serialize(const boost::any& value) {
if (value.empty()) return "";
std::ostringstream oss;
oss << boost::any_cast<T>(value);
return oss.str();
}
template <typename T>
static typename std::enable_if<!hasSerialize<T>::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<boost::any(const PropertyMap& other)> InitializerFunction;
/// function callback used to signal value setting to external components
typedef std::function<void(const Property*)> 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<T>::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 <typename T, typename = std::ostream&>
struct hasSerialize : std::false_type {};
template <typename T>
struct hasSerialize<T, decltype(std::declval<std::ostream&>() << std::declval<T>())> : std::true_type {};
template <typename T, typename = std::istream&>
struct hasDeserialize : std::false_type {};
template <typename T>
struct hasDeserialize<T, decltype(std::declval<std::istream&>() >> std::declval<T&>())> : 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 <typename T>
class PropertySerializer : protected PropertySerializerBase {
public:
PropertySerializer() {
insert(typeid(T), typeName<T>(), &serialize, &deserialize);
}
template <class Q = T>
static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type
typeName() {
return ros::message_traits::DataType<T>::value();
}
template <class Q = T>
static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type
typeName() { return typeid(T).name(); }
private:
/** Serialization based on std::[io]stringstream */
template <class Q = T>
static typename std::enable_if<hasSerialize<Q>::value, std::string>::type
serialize(const boost::any& value) {
std::ostringstream oss;
oss << boost::any_cast<T>(value);
return oss.str();
}
template <class Q = T>
static typename std::enable_if<hasSerialize<Q>::value && hasDeserialize<Q>::value, boost::any>::type
deserialize(const std::string& wired) {
std::istringstream iss(wired);
T value;
iss >> value;
return value;
}
/** No serialization available */
template <class Q = T>
static typename std::enable_if<!hasSerialize<Q>::value, std::string>::type
serialize(const boost::any& value) { return dummySerialize(value); }
template <class Q = T>
static typename std::enable_if<!hasSerialize<Q>::value || !hasDeserialize<Q>::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<std::string, Property>::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<typename T>
Property& declare(const std::string& name, const std::string& description = "") {
return declare(name, typeid(T), description, boost::any(), &Property::serialize<T>);
PropertySerializer<T>(); // register serializer/deserializer
return declare(name, typeid(T), description, boost::any());
}
/// declare a property with default value
template<typename T>
Property& declare(const std::string& name, const T& default_value,
const std::string& description = "") {
return declare(name, typeid(T), description, default_value, &Property::serialize<T>);
PropertySerializer<T>(); // register serializer/deserializer
return declare(name, typeid(T), description, default_value);
}
/// declare all given properties also in other PropertyMap

View File

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

View File

@ -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<std::type_index, Entry> RegistryMap;
RegistryMap types_;
// map from type names (type.name or ROS msg name) to entry in types_
typedef std::map<std::string, RegistryMap::iterator> 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<std::string> &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<std::string> &properties)
@ -167,8 +255,7 @@ void PropertyMap::set<boost::any>(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;
}

View File

@ -53,6 +53,9 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved in Cartesian direction");
p.declare<boost::any>("direction", "motion specification");
// register actual types
PropertySerializer<geometry_msgs::TwistStamped>();
PropertySerializer<geometry_msgs::Vector3Stamped>();
p.declare<double>("min_distance", -1.0, "minimum distance to move");
p.declare<double>("max_distance", 0.0, "maximum distance to move");

View File

@ -53,6 +53,11 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<boost::any>("goal", "goal specification");
// register actual types
PropertySerializer<std::string>();
PropertySerializer<moveit_msgs::RobotState>();
PropertySerializer<geometry_msgs::PointStamped>();
PropertySerializer<geometry_msgs::PoseStamped>();
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");

View File

@ -27,12 +27,11 @@ TEST(Property, directset) {
PropertyMap props;
props.set("int1", 1);
EXPECT_EQ(props.get<int>("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<int>("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<std::string>("any"), "foo");
}
TEST(Property, serialize_basic) {
EXPECT_TRUE(hasSerialize<int>::value);
EXPECT_TRUE(hasDeserialize<int>::value);
EXPECT_TRUE(hasSerialize<double>::value);
EXPECT_TRUE(hasDeserialize<double>::value);
EXPECT_TRUE(hasSerialize<std::string>::value);
EXPECT_TRUE(hasDeserialize<std::string>::value);
}
TEST(Property, serialize) {
PropertyMap props;
props.declare<int>("int");

View File

@ -1,3 +1,4 @@
string name
string description
string type
string value

View File

@ -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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
PRIVATE ${catkin_INCLUDE_DIRS}
PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS}
)
install(TARGETS ${MOVEIT_LIB_NAME}

View File

@ -35,32 +35,48 @@
/* Author: Robert Haschke */
#include "property_factory.h"
#include <boost/functional/factory.hpp>
#include <moveit/task_constructor/stage.h>
#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 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 <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()));
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<std::string>(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 <typename T>
static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop,
const planning_scene::PlanningScene*,
rviz::DisplayContext*) {
T value = !mtc_prop.value().empty() ? boost::any_cast<T>(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<double>(&helper<double, rviz::FloatProperty>);
registerType<QString>(&helper<QString, rviz::StringProperty>);
// register some standard types
registerType<float>(&floatFactory<float>);
registerType<double>(&floatFactory<double>);
registerType<std::string>(&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
}

View File

@ -42,43 +42,89 @@
#include <functional>
#include <typeindex>
#include <moveit/task_constructor/properties.h>
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<rviz::Property*(const QString& name, moveit::task_constructor::Property*)> FactoryFunction;
typedef std::function<rviz::Property*(const QString& name, moveit::task_constructor::Property&,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context)> PropertyFactoryFunction;
typedef std::function<rviz::PropertyTreeModel*(moveit::task_constructor::PropertyMap&,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context)> TreeFactoryFunction;
/// register a new factory function for type T
/// register a factory function for type T
template <typename T>
void registerType(const FactoryFunction& f) { registerType(std::type_index(typeid(T)).name(), f); }
inline void registerType(const PropertyFactoryFunction& f) {
moveit::task_constructor::PropertySerializer<T>(); // register serializer
registerType(moveit::task_constructor::PropertySerializer<T>::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 <typename T>
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<std::string, FactoryFunction> registry_;
std::map<std::string, PropertyFactoryFunction> property_registry_;
std::map<std::type_index, TreeFactoryFunction> 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);
}

View File

@ -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 <yaml.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/float_property.h>
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<const char*>(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<const yaml_char_t*>(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<rviz::FloatProperty*>(old)) {
bool ok = true;
double v = value.toDouble(&ok);
if (ok) p->setValue(v);
return ok;
}
if (rviz::StringProperty* p = dynamic_cast<rviz::StringProperty*>(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<rviz::FloatProperty*>(old) ||
dynamic_cast<rviz::StringProperty*>(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);
}
}

View File

@ -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_;

View File

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

View File

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

View File

@ -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 Type>
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<QString, BuiltInClassRecord>::const_iterator iter = built_ins_.find( class_id );

View File

@ -37,11 +37,13 @@
#include <stdio.h>
#include "remote_task_model.h"
#include "properties/property_factory.h"
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/properties.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>
@ -66,11 +68,12 @@ struct RemoteTaskModel::Node {
InterfaceFlags interface_flags_;
NodeFlags node_flags_;
std::unique_ptr<RemoteSolutionModel> solutions_;
std::unique_ptr<rviz::PropertyTreeModel> properties_;
std::unique_ptr<rviz::PropertyTreeModel> property_tree_;
std::map<std::string, Property> 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<moveit_task_constructor_msgs::Property>& 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<moveit_task_constructor_msgs::Property> &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 {

View File

@ -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<uint32_t, Node*> 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);

View File

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

View File

@ -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<SerialContainer>("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;
}

View File

@ -51,7 +51,10 @@
#include <QPointer>
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);

View File

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

View File

@ -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<SerialContainer>(task_name),
planning_scene::PlanningSceneConstPtr(), nullptr);
for (int i = 0; i != children; ++i)
m.add(std::make_unique<stages::CurrentState>(std::to_string(i)));
{ SCOPED_TRACE("localTaskModel"); validate(m, {"task pipeline"}); }
{ SCOPED_TRACE("localTaskModel"); validate(m, {task_name}); }
}
TEST_F(TaskListModelTest, noChildren) {