mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge PR #72: rviz property visualization
This commit is contained in:
commit
1e0a9401e7
@ -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>);
|
||||
const std::string& description = "") {
|
||||
PropertySerializer<T>(); // register serializer/deserializer
|
||||
return declare(name, typeid(T), description, default_value);
|
||||
}
|
||||
|
||||
/// declare all given properties also in other PropertyMap
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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");
|
||||
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -1,3 +1,4 @@
|
||||
string name
|
||||
string description
|
||||
string type
|
||||
string value
|
||||
|
||||
@ -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}
|
||||
|
||||
@ -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
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
@ -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_;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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 );
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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()); });
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user