PropertyMap

This commit is contained in:
Robert Haschke 2017-12-01 21:19:16 +01:00
parent 0246d60de9
commit 7d8f8dfc8d
12 changed files with 353 additions and 6 deletions

View File

@ -0,0 +1,102 @@
#pragma once
#include <boost/any.hpp>
#include <typeindex>
#include <map>
#include <set>
#include <functional>
namespace moveit {
namespace task_constructor {
class Property;
class PropertyMap;
/// initializer function, using another name from the passed property map
boost::any fromName(const PropertyMap& other, const std::string& other_name);
enum PropertyInitializerSource {
PARENT,
INTERFACE,
};
class Property {
friend class PropertyMap;
public:
typedef std::function<boost::any(const PropertyMap& other, const std::string& other_name)> InitializerFunction;
typedef std::map<PropertyInitializerSource, InitializerFunction> InitializerMap;
Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value);
void setValue(const boost::any& value);
const boost::any& value() const;
const boost::any& defaultValue() const { return default_; }
const std::string& description() const { return description_; }
std::string typeName() const { return type_index_.name(); }
/// set an initializer function
Property &initFrom(PropertyInitializerSource source, const InitializerFunction& f = fromName);
Property &initFrom(PropertyInitializerSource source, const std::string& other_name);
private:
std::string description_;
std::type_index type_index_;
boost::any default_;
boost::any value_;
InitializerMap initializers_;
};
class PropertyMap
{
std::map<std::string, Property> props_;
public:
/// declare a property for future use
Property& declare(const std::string& name, const std::type_info& type,
const std::string& description = "",
const boost::any& default_value = boost::any());
template<typename T>
Property& declare(const std::string& name, const std::string& description = "") {
return declare(name, typeid(T), description);
}
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);
}
/// get the property with given name
Property& property(const std::string &name);
const Property& property(const std::string &name) const {
return const_cast<PropertyMap*>(this)->property(name);
}
/// allow initialization from given source for listed properties
void initFrom(PropertyInitializerSource source, const std::set<std::string> &properties = {});
/// set the value of a property
void set(const std::string& name, const boost::any& value);
/// get the value of a property
const boost::any& get(const std::string& name) const;
template<typename T>
const T& get(const std::string& name) const {
const boost::any& value = get(name);
if (value.empty())
throw std::runtime_error(std::string("undefined property: " + name));
return boost::any_cast<const T&>(value);
}
/// reset properties to nil, if they have initializers
void reset();
/// init properties from initializers
void initFrom(PropertyInitializerSource source, const PropertyMap& other,
bool checkConsistency = false);
};
} // namespace task_constructor
} // namespace moveit

View File

@ -107,6 +107,12 @@ public:
/// remove function callback
void erase(SolutionCallbackList::const_iterator which);
PropertyMap& properties();
const PropertyMap& properties() const {
return const_cast<Stage*>(this)->properties();
}
void setProperty(const std::string& name, const boost::any& value);
protected:
/// Stage can only be instantiated through derived classes
Stage(StagePrivate *impl);

View File

@ -43,7 +43,7 @@ public:
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); }
/// validate that sendForward() and sendBackward() will succeed
/// validate correct configuration of this stage
/// should be only called by containers' init() method
void validate() const;
@ -61,6 +61,7 @@ public:
protected:
Stage* const me_; // associated/owning Stage instance
std::string name_;
PropertyMap properties_;
InterfacePtr starts_;
InterfacePtr ends_;
@ -121,6 +122,8 @@ protected:
// get informed when new start or end state becomes available
void newStartState(const std::list<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::iterator& it);
// initialize properties from parent and/or state
void initProperties(const InterfaceState &state);
Interface::const_iterator next_start_state_;
Interface::const_iterator next_end_state_;
@ -148,6 +151,10 @@ public:
bool canCompute() const override;
bool compute() override;
private:
// initialize properties from parent
void initProperties();
};
PIMPL_FUNCTIONS(Generator)
@ -168,6 +175,8 @@ private:
// get informed when new start or end state becomes available
void newStartState(const std::list<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::iterator& it);
// initialize properties from parent and/or interface states
void initProperties(const InterfaceState &start, const InterfaceState &end);
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
};

View File

@ -3,6 +3,7 @@
#pragma once
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h>
#include <moveit_task_constructor_msgs/Solution.h>
#include <visualization_msgs/MarkerArray.h>
@ -50,6 +51,9 @@ public:
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
PropertyMap& properties() { return properties_; }
const PropertyMap& properties() const { return properties_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
@ -57,7 +61,7 @@ private:
private:
planning_scene::PlanningSceneConstPtr scene_;
// TODO: add PropertyMap: std::map<std::string, std::any> to allow passing of parameters or attributes
PropertyMap properties_;
Solutions incoming_trajectories_;
Solutions outgoing_trajectories_;
};

View File

@ -72,6 +72,13 @@ public:
ContainerBase *stages();
const ContainerBase *stages() const;
/// properties access
PropertyMap& properties();
const PropertyMap& properties() const {
return const_cast<Task*>(this)->properties();
}
void setProperty(const std::string& name, const boost::any& value);
protected:
void initModel();
void initScene();

View File

@ -1,4 +1,5 @@
add_library(${PROJECT_NAME}
properties.cpp
storage.cpp
stage.cpp
container.cpp
@ -6,6 +7,7 @@ add_library(${PROJECT_NAME}
introspection.cpp
${PROJECT_INCLUDE}/utils.h
${PROJECT_INCLUDE}/properties.h
${PROJECT_INCLUDE}/storage.h
${PROJECT_INCLUDE}/stage.h
${PROJECT_INCLUDE}/stage_p.h
@ -14,6 +16,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/task.h
${PROJECT_INCLUDE}/introspection.h
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_include_directories(${PROJECT_NAME}
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>

140
core/src/properties.cpp Normal file
View File

@ -0,0 +1,140 @@
#include <moveit/task_constructor/properties.h>
#include <boost/format.hpp>
#include <ros/console.h>
namespace moveit {
namespace task_constructor {
Property::Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value)
: description_(description)
, type_index_(type_index)
, default_(default_value)
{
if (!default_.empty() && std::type_index(default_.type()) != type_index_)
throw std::runtime_error("type of default value doesn't match declared type");
}
namespace {
void typeCheck(const boost::any& value, const std::type_index& type_index)
{
if (std::type_index(value.type()) != type_index) {
static boost::format fmt("type (%1%) doesn't match property's declared type (%2%)");
throw std::runtime_error(boost::str(fmt % value.type().name() % type_index.name()));
}
}
}
void Property::setValue(const boost::any &value) {
typeCheck(value, type_index_);
value_ = value;
// clear all initializers when value was explicitly set
initializers_.clear();
}
const boost::any &Property::value() const {
return value_.empty() ? default_ : value_;
}
Property& Property::initFrom(PropertyInitializerSource source, const Property::InitializerFunction &f)
{
initializers_[source] = f;
return *this;
}
Property& Property::initFrom(PropertyInitializerSource source, const std::string &other_name)
{
initializers_[source] = [other_name](const PropertyMap& other, const std::string&) {
return fromName(other, other_name);
};
return *this;
}
Property& PropertyMap::declare(const std::string &name, const std::type_info &type,
const std::string &description, const boost::any &default_value)
{
auto it_inserted = props_.insert(std::make_pair(name, Property(std::type_index(type), description, default_value)));
if (!it_inserted.second && std::type_index(type) != it_inserted.first->second.type_index_)
throw std::runtime_error("Property '" + name + "' was already declared with different type.");
return it_inserted.first->second;
}
Property& PropertyMap::property(const std::string &name)
{
auto it = props_.find(name);
if (it == props_.end())
throw std::runtime_error("Unknown property '" + name + "'");
return it->second;
}
void PropertyMap::initFrom(PropertyInitializerSource source, const std::set<std::string> &properties)
{
for (auto &pair : props_) {
if (properties.empty() || properties.count(pair.first))
pair.second.initFrom(source);
}
}
void PropertyMap::set(const std::string &name, const boost::any &value)
{
auto range = props_.equal_range(name);
if (range.first == range.second) { // name is not yet declared
if (value.empty()) {
ROS_ERROR("trying to define property '%s' with NULL value", name.c_str());
return;
}
auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any())));
it->second.setValue(value);
} else {
assert(range.first->first == name);
range.first->second.setValue(value);
}
}
const boost::any &PropertyMap::get(const std::string &name) const
{
return property(name).value();
}
void PropertyMap::reset()
{
for (auto& pair : props_) {
Property &p = pair.second;
if (!p.initializers_.empty())
// if there are initializers, reset property value
// explicitly setting the value, clears initializers
p.value_ = boost::any();
}
}
void PropertyMap::initFrom(PropertyInitializerSource source, const PropertyMap &other,
bool checkConsistency)
{
for (auto& pair : props_) {
Property &p = pair.second;
auto it = p.initializers_.find(source);
if (it == p.initializers_.end()) continue;
const boost::any& value = it->second(other, pair.first);
if (value.empty()) continue;
typeCheck(value, p.type_index_);
#if 0 // TODO: we cannot generically check for equality of boost::anys
if (checkConsistency && !p.value_.empty() && p.value_ != value)
throw std::runtime_error ("inconsistent values for property '" + pair.first + "'");
#endif
p.value_ = value;
}
}
boost::any fromName(const PropertyMap& other, const std::string& other_name)
{
try {
return other.get(other_name);
} catch (const std::runtime_error &e) {
return boost::any();
}
}
} // namespace task_constructor
} // namespace moveit

View File

@ -54,9 +54,11 @@ void StagePrivate::validate() const {
InitStageException errors;
InterfaceFlags f = interfaceFlags();
// validate that sendForward() will succeed
if (!implies(f & WRITES_NEXT_START, bool(nextStarts())))
errors.push_back(*me_, "sends forward, but next stage cannot receive");
// validate that sendBackward() will succeed
if (!implies(f & WRITES_PREV_END, bool(prevEnds())))
errors.push_back(*me_, "sends backward, but previous stage cannot receive");
@ -124,6 +126,15 @@ void Stage::erase(SolutionCallbackList::const_iterator which)
pimpl()->solution_cbs_.erase(which);
}
PropertyMap &Stage::properties()
{
return pimpl()->properties_;
}
void Stage::setProperty(const std::string& name, const boost::any& value) {
pimpl()->properties_.set(name, value);
}
template<InterfaceFlag own, InterfaceFlag other>
const char* direction(const StagePrivate& stage) {
InterfaceFlags f = stage.interfaceFlags();
@ -226,6 +237,13 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
return state;
}
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
{
properties_.reset();
properties_.initFrom(PARENT, parent()->properties());
properties_.initFrom(INTERFACE, state.properties());
}
bool PropagatingEitherWayPrivate::canCompute() const
{
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
@ -241,11 +259,15 @@ bool PropagatingEitherWayPrivate::compute()
bool result = false;
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
if (me->computeForward(fetchStartState()))
const InterfaceState& state = fetchStartState();
initProperties(state);
if (me->computeForward(state))
result |= true;
}
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
if (me->computeBackward(fetchEndState()))
const InterfaceState& state = fetchEndState();
initProperties(state);
if (me->computeBackward(state))
result |= true;
}
return result;
@ -404,9 +426,16 @@ bool GeneratorPrivate::canCompute() const {
}
bool GeneratorPrivate::compute() {
initProperties();
return static_cast<Generator*>(me_)->compute();
}
void GeneratorPrivate::initProperties()
{
properties_.reset();
properties_.initFrom(PARENT, parent()->properties());
}
Generator::Generator(const std::string &name)
: ComputeBase(new GeneratorPrivate(this, name))
@ -450,6 +479,15 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it)
--it_pairs_.second;
}
void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end)
{
properties_.reset();
properties_.initFrom(PARENT, parent()->properties());
// properties from start/end states need to be consistent to each other
properties_.initFrom(INTERFACE, start.properties());
properties_.initFrom(INTERFACE, end.properties(), true);
}
bool ConnectingPrivate::canCompute() const{
// TODO: implement this properly
return it_pairs_.first != starts_->end() &&
@ -460,6 +498,7 @@ bool ConnectingPrivate::compute() {
// TODO: implement this properly
const InterfaceState& from = *it_pairs_.first;
const InterfaceState& to = *(it_pairs_.second++);
initProperties(from, to);
return static_cast<Connecting*>(me_)->compute(from, to);
}

View File

@ -13,7 +13,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
}
InterfaceState::InterfaceState(const InterfaceState &existing)
: scene_(existing.scene())
: scene_(existing.scene_), properties_(existing.properties_)
{
}

View File

@ -215,6 +215,18 @@ const ContainerBase* Task::stages() const
return const_cast<Task*>(this)->stages();
}
PropertyMap &Task::properties()
{
// forward to wrapped() stage
return wrapped()->properties();
}
void Task::setProperty(const std::string &name, const boost::any &value)
{
// forward to wrapped() stage
wrapped()->setProperty(name, value);
}
std::string Task::id() const
{
return id_;

View File

@ -7,8 +7,11 @@ if (CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
target_link_libraries(${PROJECT_NAME}-test-container
${PROJECT_NAME}
${catkin_LIBRARIES}
gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
target_link_libraries(${PROJECT_NAME}-test-properties
${PROJECT_NAME} gtest_main)
endif()
add_executable(test_plan_current_state test_plan_current_state.cpp)

View File

@ -0,0 +1,22 @@
#include <moveit/task_constructor/properties.h>
#include <gtest/gtest.h>
#include <initializer_list>
using namespace moveit::task_constructor;
TEST(Property, serialContainer) {
PropertyMap props;
props.declare<double>("double1");
props.declare<double>("double2", 1);
// avoid second declaration with different type
props.declare<double>("double1");
ASSERT_THROW(props.declare<long double>("double1"), std::runtime_error);
// types not matching?
ASSERT_THROW(props.set("double1", 1), std::runtime_error);
props.set("double1", 3.14);
ASSERT_EQ(props.get<double>("double1"), 3.14);
}