mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
4db7e8eb88
commit
c9e3be08d0
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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{
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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");
|
||||
|
||||
Loading…
Reference in New Issue
Block a user