diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h new file mode 100644 index 00000000..3f7a1aa9 --- /dev/null +++ b/core/include/moveit/task_constructor/properties.h @@ -0,0 +1,102 @@ +#pragma once + +#include +#include +#include +#include +#include + +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 InitializerFunction; + typedef std::map 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 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 + Property& declare(const std::string& name, const std::string& description = "") { + return declare(name, typeid(T), description); + } + template + 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(this)->property(name); + } + + /// allow initialization from given source for listed properties + void initFrom(PropertyInitializerSource source, const std::set &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 + 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(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 diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index e422a37d..8a18ed1f 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -107,6 +107,12 @@ public: /// remove function callback void erase(SolutionCallbackList::const_iterator which); + PropertyMap& properties(); + const PropertyMap& properties() const { + return const_cast(this)->properties(); + } + void setProperty(const std::string& name, const boost::any& value); + protected: /// Stage can only be instantiated through derived classes Stage(StagePrivate *impl); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 2e42d1ee..6fca3728 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -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::iterator& it); void newEndState(const std::list::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::iterator& it); void newEndState(const std::list::iterator& it); + // initialize properties from parent and/or interface states + void initProperties(const InterfaceState &start, const InterfaceState &end); std::pair it_pairs_; }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 826fd19b..6a4310d8 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -3,6 +3,7 @@ #pragma once #include +#include #include #include @@ -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 to allow passing of parameters or attributes + PropertyMap properties_; Solutions incoming_trajectories_; Solutions outgoing_trajectories_; }; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index d15a4455..c1ebe1e9 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -72,6 +72,13 @@ public: ContainerBase *stages(); const ContainerBase *stages() const; + /// properties access + PropertyMap& properties(); + const PropertyMap& properties() const { + return const_cast(this)->properties(); + } + void setProperty(const std::string& name, const boost::any& value); + protected: void initModel(); void initScene(); diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 0608c642..db55946f 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -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 $ PUBLIC $ diff --git a/core/src/properties.cpp b/core/src/properties.cpp new file mode 100644 index 00000000..1795acdc --- /dev/null +++ b/core/src/properties.cpp @@ -0,0 +1,140 @@ +#include +#include +#include + +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 &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 diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b632f5ab..aaf92772 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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 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(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(me_)->compute(from, to); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 2bf3f7d1..4735914c 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -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_) { } diff --git a/core/src/task.cpp b/core/src/task.cpp index 409b5563..e3ca4aea 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -215,6 +215,18 @@ const ContainerBase* Task::stages() const return const_cast(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_; diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 0b18a0cf..e12b1081 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -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) diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp new file mode 100644 index 00000000..f8843964 --- /dev/null +++ b/core/test/test_properties.cpp @@ -0,0 +1,22 @@ +#include + +#include +#include + +using namespace moveit::task_constructor; + +TEST(Property, serialContainer) { + PropertyMap props; + props.declare("double1"); + props.declare("double2", 1); + + // avoid second declaration with different type + props.declare("double1"); + ASSERT_THROW(props.declare("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("double1"), 3.14); +}