fix initialization order of properties

First from INTERFACE, second from PARENT.

INTERFACE initialization only makes sense for Propagating stages.
Connecting stages should ensure that interfaces define identical
properties which is not possible with boost::any.
This commit is contained in:
Robert Haschke 2018-02-03 15:28:25 +01:00
parent 4db7e8eb88
commit c9e3be08d0
6 changed files with 35 additions and 11 deletions

View File

@ -150,6 +150,8 @@ public:
/// get the value of a property
const boost::any& get(const std::string& name) const;
/// Get typed value of property. Throws runtime_error if undefined or bad_any_cast on type mismatch.
template<typename T>
const T& get(const std::string& name) const {
const boost::any& value = get(name);
@ -157,6 +159,12 @@ public:
throw std::runtime_error(std::string("undefined property: " + name));
return boost::any_cast<const T&>(value);
}
/// get typed value of property, using fallback if undefined. Throws bad_any_cast on type mismatch.
template<typename T>
const T& get(const std::string& name, const T& fallback) const {
const boost::any& value = get(name);
return (value.empty()) ? fallback : boost::any_cast<const T&>(value);
}
/// count number of defined properties from given list
size_t countDefined(const std::vector<std::string>& list) const;

View File

@ -73,6 +73,7 @@ private:
std::ostream& operator<<(std::ostream &os, const InitStageException& e);
class ContainerBase;
class StagePrivate;
class Stage {
friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
@ -102,6 +103,7 @@ public:
/// initialize stage once before planning
virtual void init(const planning_scene::PlanningSceneConstPtr& scene);
const ContainerBase* parent() const;
const std::string& name() const;
void setName(const std::string& name);
virtual size_t numSolutions() const = 0;

View File

@ -127,10 +127,16 @@ void ContainerBase::reset()
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
{
InitStageException errors;
auto& children = pimpl()->children();
auto impl = pimpl();
auto& children = impl->children();
Stage::init(scene);
// containers don't need to reset and init their properties on each execution
impl->properties_.reset();
if (impl->parent())
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
// we need to have some children to do the actual work
if (children.empty()) {
errors.push_back(*this, "no children");

View File

@ -105,6 +105,10 @@ void Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
{
}
const ContainerBase *Stage::parent() const {
return pimpl_->parent_;
}
const std::string& Stage::name() const {
return pimpl_->name_;
}
@ -239,9 +243,12 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
{
// reset properties to their defaults
properties_.reset();
properties_.performInitFrom(Stage::PARENT, parent()->properties());
// first init from INTERFACE
properties_.performInitFrom(Stage::INTERFACE, state.properties());
// then init from PARENT
properties_.performInitFrom(Stage::PARENT, parent()->properties());
}
bool PropagatingEitherWayPrivate::canCompute() const
@ -432,7 +439,9 @@ bool GeneratorPrivate::compute() {
void GeneratorPrivate::initProperties()
{
// reset properties to their defaults
properties_.reset();
// then init from PARENT
properties_.performInitFrom(Stage::PARENT, parent()->properties());
}
@ -481,11 +490,10 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it)
void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end)
{
// reset properties to their defaults
properties_.reset();
// then init from PARENT
properties_.performInitFrom(Stage::PARENT, parent()->properties());
// properties from start/end states need to be consistent to each other
properties_.performInitFrom(Stage::INTERFACE, start.properties());
properties_.performInitFrom(Stage::INTERFACE, end.properties(), true);
}
bool ConnectingPrivate::canCompute() const{

View File

@ -17,7 +17,7 @@ CartesianPositionMotion::CartesianPositionMotion(std::string name)
{
auto& p = properties();
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("link", "", "name of link used for IK");
p.declare<std::string>("link", "name of link used for IK");
p.declare<double>("min_distance", "minimum distance to move");
p.declare<double>("max_distance", "maximum distance to move");
p.declare<double>("step_size", 0.005);

View File

@ -18,9 +18,9 @@ Gripper::Gripper(std::string name)
{
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("link", "", "name of link the eef is attached to");
p.declare<std::string>("named_target", "", "named target in eef group");
p.declare<std::string>("grasp_object", "", "name of grasp object");
p.declare<std::string>("link", "name of link the eef is attached to");
p.declare<std::string>("named_target", "named target in eef group");
p.declare<std::string>("grasp_object", "name of grasp object");
}
void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
@ -58,9 +58,9 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
std::string link = props.get<std::string>("link");
std::string link = props.get<std::string>("link", "");
const std::string& named_target = props.get<std::string>("named_target");
const std::string& grasp_object = props.get<std::string>("grasp_object");
const std::string& grasp_object = props.get<std::string>("grasp_object", "");
if(!mgi_){
assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");