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
|
/// remove function callback
|
||||||
void erase(SolutionCallbackList::const_iterator which);
|
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:
|
protected:
|
||||||
/// Stage can only be instantiated through derived classes
|
/// Stage can only be instantiated through derived classes
|
||||||
Stage(StagePrivate *impl);
|
Stage(StagePrivate *impl);
|
||||||
|
|||||||
@ -43,7 +43,7 @@ public:
|
|||||||
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
||||||
inline InterfaceConstPtr nextStarts() const { return next_starts_.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
|
/// should be only called by containers' init() method
|
||||||
void validate() const;
|
void validate() const;
|
||||||
|
|
||||||
@ -61,6 +61,7 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
Stage* const me_; // associated/owning Stage instance
|
Stage* const me_; // associated/owning Stage instance
|
||||||
std::string name_;
|
std::string name_;
|
||||||
|
PropertyMap properties_;
|
||||||
|
|
||||||
InterfacePtr starts_;
|
InterfacePtr starts_;
|
||||||
InterfacePtr ends_;
|
InterfacePtr ends_;
|
||||||
@ -121,6 +122,8 @@ protected:
|
|||||||
// get informed when new start or end state becomes available
|
// get informed when new start or end state becomes available
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
void newEndState(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_start_state_;
|
||||||
Interface::const_iterator next_end_state_;
|
Interface::const_iterator next_end_state_;
|
||||||
@ -148,6 +151,10 @@ public:
|
|||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
// initialize properties from parent
|
||||||
|
void initProperties();
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Generator)
|
PIMPL_FUNCTIONS(Generator)
|
||||||
|
|
||||||
@ -168,6 +175,8 @@ private:
|
|||||||
// get informed when new start or end state becomes available
|
// get informed when new start or end state becomes available
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
void newEndState(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_;
|
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -3,6 +3,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
#include <moveit/task_constructor/properties.h>
|
||||||
#include <moveit_task_constructor_msgs/Solution.h>
|
#include <moveit_task_constructor_msgs/Solution.h>
|
||||||
#include <visualization_msgs/MarkerArray.h>
|
#include <visualization_msgs/MarkerArray.h>
|
||||||
|
|
||||||
@ -50,6 +51,9 @@ public:
|
|||||||
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
|
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
|
||||||
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
|
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
|
||||||
|
|
||||||
|
PropertyMap& properties() { return properties_; }
|
||||||
|
const PropertyMap& properties() const { return properties_; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// these methods should be only called by SolutionBase::set[Start|End]State()
|
// these methods should be only called by SolutionBase::set[Start|End]State()
|
||||||
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
||||||
@ -57,7 +61,7 @@ private:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
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 incoming_trajectories_;
|
||||||
Solutions outgoing_trajectories_;
|
Solutions outgoing_trajectories_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -72,6 +72,13 @@ public:
|
|||||||
ContainerBase *stages();
|
ContainerBase *stages();
|
||||||
const ContainerBase *stages() const;
|
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:
|
protected:
|
||||||
void initModel();
|
void initModel();
|
||||||
void initScene();
|
void initScene();
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
add_library(${PROJECT_NAME}
|
add_library(${PROJECT_NAME}
|
||||||
|
properties.cpp
|
||||||
storage.cpp
|
storage.cpp
|
||||||
stage.cpp
|
stage.cpp
|
||||||
container.cpp
|
container.cpp
|
||||||
@ -6,6 +7,7 @@ add_library(${PROJECT_NAME}
|
|||||||
introspection.cpp
|
introspection.cpp
|
||||||
|
|
||||||
${PROJECT_INCLUDE}/utils.h
|
${PROJECT_INCLUDE}/utils.h
|
||||||
|
${PROJECT_INCLUDE}/properties.h
|
||||||
${PROJECT_INCLUDE}/storage.h
|
${PROJECT_INCLUDE}/storage.h
|
||||||
${PROJECT_INCLUDE}/stage.h
|
${PROJECT_INCLUDE}/stage.h
|
||||||
${PROJECT_INCLUDE}/stage_p.h
|
${PROJECT_INCLUDE}/stage_p.h
|
||||||
@ -14,6 +16,7 @@ add_library(${PROJECT_NAME}
|
|||||||
${PROJECT_INCLUDE}/task.h
|
${PROJECT_INCLUDE}/task.h
|
||||||
${PROJECT_INCLUDE}/introspection.h
|
${PROJECT_INCLUDE}/introspection.h
|
||||||
)
|
)
|
||||||
|
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
target_include_directories(${PROJECT_NAME}
|
target_include_directories(${PROJECT_NAME}
|
||||||
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
||||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
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;
|
InitStageException errors;
|
||||||
|
|
||||||
InterfaceFlags f = interfaceFlags();
|
InterfaceFlags f = interfaceFlags();
|
||||||
|
// validate that sendForward() will succeed
|
||||||
if (!implies(f & WRITES_NEXT_START, bool(nextStarts())))
|
if (!implies(f & WRITES_NEXT_START, bool(nextStarts())))
|
||||||
errors.push_back(*me_, "sends forward, but next stage cannot receive");
|
errors.push_back(*me_, "sends forward, but next stage cannot receive");
|
||||||
|
|
||||||
|
// validate that sendBackward() will succeed
|
||||||
if (!implies(f & WRITES_PREV_END, bool(prevEnds())))
|
if (!implies(f & WRITES_PREV_END, bool(prevEnds())))
|
||||||
errors.push_back(*me_, "sends backward, but previous stage cannot receive");
|
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);
|
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>
|
template<InterfaceFlag own, InterfaceFlag other>
|
||||||
const char* direction(const StagePrivate& stage) {
|
const char* direction(const StagePrivate& stage) {
|
||||||
InterfaceFlags f = stage.interfaceFlags();
|
InterfaceFlags f = stage.interfaceFlags();
|
||||||
@ -226,6 +237,13 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
|||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
|
||||||
|
{
|
||||||
|
properties_.reset();
|
||||||
|
properties_.initFrom(PARENT, parent()->properties());
|
||||||
|
properties_.initFrom(INTERFACE, state.properties());
|
||||||
|
}
|
||||||
|
|
||||||
bool PropagatingEitherWayPrivate::canCompute() const
|
bool PropagatingEitherWayPrivate::canCompute() const
|
||||||
{
|
{
|
||||||
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
|
||||||
@ -241,11 +259,15 @@ bool PropagatingEitherWayPrivate::compute()
|
|||||||
|
|
||||||
bool result = false;
|
bool result = false;
|
||||||
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
|
||||||
if (me->computeForward(fetchStartState()))
|
const InterfaceState& state = fetchStartState();
|
||||||
|
initProperties(state);
|
||||||
|
if (me->computeForward(state))
|
||||||
result |= true;
|
result |= true;
|
||||||
}
|
}
|
||||||
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
|
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
|
||||||
if (me->computeBackward(fetchEndState()))
|
const InterfaceState& state = fetchEndState();
|
||||||
|
initProperties(state);
|
||||||
|
if (me->computeBackward(state))
|
||||||
result |= true;
|
result |= true;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
@ -404,9 +426,16 @@ bool GeneratorPrivate::canCompute() const {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool GeneratorPrivate::compute() {
|
bool GeneratorPrivate::compute() {
|
||||||
|
initProperties();
|
||||||
return static_cast<Generator*>(me_)->compute();
|
return static_cast<Generator*>(me_)->compute();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void GeneratorPrivate::initProperties()
|
||||||
|
{
|
||||||
|
properties_.reset();
|
||||||
|
properties_.initFrom(PARENT, parent()->properties());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
Generator::Generator(const std::string &name)
|
Generator::Generator(const std::string &name)
|
||||||
: ComputeBase(new GeneratorPrivate(this, name))
|
: ComputeBase(new GeneratorPrivate(this, name))
|
||||||
@ -450,6 +479,15 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it)
|
|||||||
--it_pairs_.second;
|
--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{
|
bool ConnectingPrivate::canCompute() const{
|
||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
return it_pairs_.first != starts_->end() &&
|
return it_pairs_.first != starts_->end() &&
|
||||||
@ -460,6 +498,7 @@ bool ConnectingPrivate::compute() {
|
|||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
const InterfaceState& from = *it_pairs_.first;
|
const InterfaceState& from = *it_pairs_.first;
|
||||||
const InterfaceState& to = *(it_pairs_.second++);
|
const InterfaceState& to = *(it_pairs_.second++);
|
||||||
|
initProperties(from, to);
|
||||||
return static_cast<Connecting*>(me_)->compute(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)
|
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();
|
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
|
std::string Task::id() const
|
||||||
{
|
{
|
||||||
return id_;
|
return id_;
|
||||||
|
|||||||
@ -7,8 +7,11 @@ if (CATKIN_ENABLE_TESTING)
|
|||||||
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-container
|
target_link_libraries(${PROJECT_NAME}-test-container
|
||||||
${PROJECT_NAME}
|
${PROJECT_NAME}
|
||||||
${catkin_LIBRARIES}
|
|
||||||
gtest_main)
|
gtest_main)
|
||||||
|
|
||||||
|
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
|
||||||
|
target_link_libraries(${PROJECT_NAME}-test-properties
|
||||||
|
${PROJECT_NAME} gtest_main)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
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