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
|
/// get the value of a property
|
||||||
const boost::any& get(const std::string& name) const;
|
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>
|
template<typename T>
|
||||||
const T& get(const std::string& name) const {
|
const T& get(const std::string& name) const {
|
||||||
const boost::any& value = get(name);
|
const boost::any& value = get(name);
|
||||||
@ -157,6 +159,12 @@ public:
|
|||||||
throw std::runtime_error(std::string("undefined property: " + name));
|
throw std::runtime_error(std::string("undefined property: " + name));
|
||||||
return boost::any_cast<const T&>(value);
|
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
|
/// count number of defined properties from given list
|
||||||
size_t countDefined(const std::vector<std::string>& list) const;
|
size_t countDefined(const std::vector<std::string>& list) const;
|
||||||
|
|||||||
@ -73,6 +73,7 @@ private:
|
|||||||
std::ostream& operator<<(std::ostream &os, const InitStageException& e);
|
std::ostream& operator<<(std::ostream &os, const InitStageException& e);
|
||||||
|
|
||||||
|
|
||||||
|
class ContainerBase;
|
||||||
class StagePrivate;
|
class StagePrivate;
|
||||||
class Stage {
|
class Stage {
|
||||||
friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
|
friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
|
||||||
@ -102,6 +103,7 @@ public:
|
|||||||
/// initialize stage once before planning
|
/// initialize stage once before planning
|
||||||
virtual void init(const planning_scene::PlanningSceneConstPtr& scene);
|
virtual void init(const planning_scene::PlanningSceneConstPtr& scene);
|
||||||
|
|
||||||
|
const ContainerBase* parent() const;
|
||||||
const std::string& name() const;
|
const std::string& name() const;
|
||||||
void setName(const std::string& name);
|
void setName(const std::string& name);
|
||||||
virtual size_t numSolutions() const = 0;
|
virtual size_t numSolutions() const = 0;
|
||||||
|
|||||||
@ -127,10 +127,16 @@ void ContainerBase::reset()
|
|||||||
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
{
|
{
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
auto& children = pimpl()->children();
|
auto impl = pimpl();
|
||||||
|
auto& children = impl->children();
|
||||||
|
|
||||||
Stage::init(scene);
|
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
|
// we need to have some children to do the actual work
|
||||||
if (children.empty()) {
|
if (children.empty()) {
|
||||||
errors.push_back(*this, "no children");
|
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 {
|
const std::string& Stage::name() const {
|
||||||
return pimpl_->name_;
|
return pimpl_->name_;
|
||||||
}
|
}
|
||||||
@ -239,9 +243,12 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
|||||||
|
|
||||||
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
|
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
|
||||||
{
|
{
|
||||||
|
// reset properties to their defaults
|
||||||
properties_.reset();
|
properties_.reset();
|
||||||
properties_.performInitFrom(Stage::PARENT, parent()->properties());
|
// first init from INTERFACE
|
||||||
properties_.performInitFrom(Stage::INTERFACE, state.properties());
|
properties_.performInitFrom(Stage::INTERFACE, state.properties());
|
||||||
|
// then init from PARENT
|
||||||
|
properties_.performInitFrom(Stage::PARENT, parent()->properties());
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PropagatingEitherWayPrivate::canCompute() const
|
bool PropagatingEitherWayPrivate::canCompute() const
|
||||||
@ -432,7 +439,9 @@ bool GeneratorPrivate::compute() {
|
|||||||
|
|
||||||
void GeneratorPrivate::initProperties()
|
void GeneratorPrivate::initProperties()
|
||||||
{
|
{
|
||||||
|
// reset properties to their defaults
|
||||||
properties_.reset();
|
properties_.reset();
|
||||||
|
// then init from PARENT
|
||||||
properties_.performInitFrom(Stage::PARENT, parent()->properties());
|
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)
|
void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end)
|
||||||
{
|
{
|
||||||
|
// reset properties to their defaults
|
||||||
properties_.reset();
|
properties_.reset();
|
||||||
|
// then init from PARENT
|
||||||
properties_.performInitFrom(Stage::PARENT, parent()->properties());
|
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{
|
bool ConnectingPrivate::canCompute() const{
|
||||||
|
|||||||
@ -17,7 +17,7 @@ CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
|||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<std::string>("group", "name of planning group");
|
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>("min_distance", "minimum distance to move");
|
||||||
p.declare<double>("max_distance", "maximum distance to move");
|
p.declare<double>("max_distance", "maximum distance to move");
|
||||||
p.declare<double>("step_size", 0.005);
|
p.declare<double>("step_size", 0.005);
|
||||||
|
|||||||
@ -18,9 +18,9 @@ Gripper::Gripper(std::string name)
|
|||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<std::string>("eef", "name of end-effector group");
|
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>("link", "name of link the eef is attached to");
|
||||||
p.declare<std::string>("named_target", "", "named target in eef group");
|
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>("grasp_object", "name of grasp object");
|
||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
|
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 auto& props = properties();
|
||||||
const std::string& eef = props.get<std::string>("eef");
|
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& 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_){
|
if(!mgi_){
|
||||||
assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");
|
assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user