diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 3be6fb80..8bc849c9 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -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 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(value); } + /// get typed value of property, using fallback if undefined. Throws bad_any_cast on type mismatch. + template + const T& get(const std::string& name, const T& fallback) const { + const boost::any& value = get(name); + return (value.empty()) ? fallback : boost::any_cast(value); + } /// count number of defined properties from given list size_t countDefined(const std::vector& list) const; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index b43ad309..2074e884 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -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; diff --git a/core/src/container.cpp b/core/src/container.cpp index fbcb12b4..31c79d89 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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"); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 718f6cd9..e9c34c0d 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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{ diff --git a/core/src/stages/cartesian_position_motion.cpp b/core/src/stages/cartesian_position_motion.cpp index 17b49ed4..9fd7ae9a 100644 --- a/core/src/stages/cartesian_position_motion.cpp +++ b/core/src/stages/cartesian_position_motion.cpp @@ -17,7 +17,7 @@ CartesianPositionMotion::CartesianPositionMotion(std::string name) { auto& p = properties(); p.declare("group", "name of planning group"); - p.declare("link", "", "name of link used for IK"); + p.declare("link", "name of link used for IK"); p.declare("min_distance", "minimum distance to move"); p.declare("max_distance", "maximum distance to move"); p.declare("step_size", 0.005); diff --git a/core/src/stages/gripper.cpp b/core/src/stages/gripper.cpp index 331e2485..f9a37d90 100644 --- a/core/src/stages/gripper.cpp +++ b/core/src/stages/gripper.cpp @@ -18,9 +18,9 @@ Gripper::Gripper(std::string name) { auto& p = properties(); p.declare("eef", "name of end-effector group"); - p.declare("link", "", "name of link the eef is attached to"); - p.declare("named_target", "", "named target in eef group"); - p.declare("grasp_object", "", "name of grasp object"); + p.declare("link", "name of link the eef is attached to"); + p.declare("named_target", "named target in eef group"); + p.declare("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("eef"); - std::string link = props.get("link"); + std::string link = props.get("link", ""); const std::string& named_target = props.get("named_target"); - const std::string& grasp_object = props.get("grasp_object"); + const std::string& grasp_object = props.get("grasp_object", ""); if(!mgi_){ assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");