mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
PropertyMap
This commit is contained in:
parent
0246d60de9
commit
7d8f8dfc8d
102
core/include/moveit/task_constructor/properties.h
Normal file
102
core/include/moveit/task_constructor/properties.h
Normal 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
|
||||
@ -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);
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
140
core/src/properties.cpp
Normal 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
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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_)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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)
|
||||
|
||||
22
core/test/test_properties.cpp
Normal file
22
core/test/test_properties.cpp
Normal 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);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user