separate reset() + init()

deducedFlags() -> interfaceFlags()
removed announcedFlags()
This commit is contained in:
Robert Haschke 2017-10-20 00:08:15 +02:00
parent 43bcf99222
commit f103341356
7 changed files with 52 additions and 70 deletions

View File

@ -37,6 +37,7 @@ public:
PRIVATE_CLASS(SerialContainer) PRIVATE_CLASS(SerialContainer)
SerialContainer(const std::string& name); SerialContainer(const std::string& name);
void reset() override;
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const planning_scene::PlanningSceneConstPtr &scene) override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;

View File

@ -89,6 +89,8 @@ public:
operator StagePrivate*(); operator StagePrivate*();
operator const StagePrivate*() const; operator const StagePrivate*() const;
/// reset stage, clearing all solutions, interfaces, etc.
virtual void reset();
/// 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);
@ -109,7 +111,7 @@ class ComputeBasePrivate;
class ComputeBase : public Stage { class ComputeBase : public Stage {
public: public:
PRIVATE_CLASS(ComputeBase) PRIVATE_CLASS(ComputeBase)
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void reset() override;
virtual size_t numSolutions() const override; virtual size_t numSolutions() const override;
protected: protected:

View File

@ -100,14 +100,6 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
pending_forward_.reset(new Interface(Interface::NotifyFunction())); pending_forward_.reset(new Interface(Interface::NotifyFunction()));
} }
InterfaceFlags SerialContainerPrivate::announcedFlags() const {
InterfaceFlags f;
if (children().empty()) return f;
f |= children().front()->pimpl()->announcedFlags() & INPUT_IF_MASK;
f |= children().back()->pimpl()->announcedFlags() & OUTPUT_IF_MASK;
return f;
}
inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const
{ {
assert(it != children().cbegin()); assert(it != children().cbegin());
@ -226,8 +218,22 @@ SerialContainer::SerialContainer(SerialContainerPrivate *impl)
SerialContainer::SerialContainer(const std::string &name) SerialContainer::SerialContainer(const std::string &name)
: SerialContainer(new SerialContainerPrivate(this, name)) : SerialContainer(new SerialContainerPrivate(this, name))
{} {}
PIMPL_FUNCTIONS(SerialContainer) PIMPL_FUNCTIONS(SerialContainer)
void SerialContainer::reset()
{
auto impl = pimpl();
// clear queues
impl->internal_to_my_starts_.clear();
impl->internal_to_my_ends_.clear();
impl->solutions_.clear();
// recursively reset children
for (auto& child: impl->children())
child->reset();
}
void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
prev->setNextStarts(next->starts()); prev->setNextStarts(next->starts());
next->setPrevEnds(prev->ends()); next->setPrevEnds(prev->ends());
@ -236,23 +242,7 @@ void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
InitStageException errors; InitStageException errors;
auto impl = pimpl(); auto impl = pimpl();
// clear queues
impl->internal_to_my_starts_.clear();
impl->internal_to_my_ends_.clear();
impl->solutions_.clear();
// recursively init all children
for (auto& stage : impl->children()) {
try {
// should be alled by derived classes too, but you never know...
stage->Stage::init(scene);
stage->init(scene);
} catch (InitStageException &e) {
errors.append(e);
}
}
// we need to have some children to do the actual work // we need to have some children to do the actual work
if (impl->children().empty()) { if (impl->children().empty()) {
@ -271,7 +261,8 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
})); }));
auto last = --impl->children().end(); auto last = --impl->children().end();
if ((*cur)->pimpl()->ends()) child_impl = **last;
if (child_impl->ends())
impl->ends_.reset(new Interface([impl, child_impl](const Interface::iterator& internal){ impl->ends_.reset(new Interface([impl, child_impl](const Interface::iterator& internal){
// new state in our ends_ interface is copied to last child, remembering the link // new state in our ends_ interface is copied to last child, remembering the link
auto it = child_impl->ends()->clone(*internal); auto it = child_impl->ends()->clone(*internal);
@ -296,15 +287,25 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
impl->connect(**prev, **cur); impl->connect(**prev, **cur);
} }
// validate connectivity of chain // recursively init all children
for (const Stage::pointer& stage : impl->children()) { for (auto& child : impl->children()) {
try { try {
stage->pimpl()->validate(); child->init(scene);
} catch (InitStageException &e) { } catch (InitStageException &e) {
errors.append(e); errors.append(e);
} }
} }
// validate connectivity of chain
for (auto& child : impl->children()) {
try {
child->pimpl()->validate();
} catch (InitStageException &e) {
errors.append(e);
}
}
std::cerr << errors;
if (errors) if (errors)
throw errors; throw errors;
} }

View File

@ -64,7 +64,6 @@ class SerialContainerPrivate : public ContainerBasePrivate {
public: public:
SerialContainerPrivate(SerialContainer* me, const std::string &name); SerialContainerPrivate(SerialContainer* me, const std::string &name);
InterfaceFlags announcedFlags() const override;
void onNewSolution(SolutionBase &s) override; void onNewSolution(SolutionBase &s) override;
void storeNewSolution(std::vector<const SolutionBase *> &&s, double cost); void storeNewSolution(std::vector<const SolutionBase *> &&s, double cost);
const std::list<SerialSolution>& solutions() const { return solutions_; } const std::list<SerialSolution>& solutions() const { return solutions_; }

View File

@ -40,19 +40,10 @@ StagePrivate::StagePrivate(Stage *me, const std::string &name)
{} {}
InterfaceFlags StagePrivate::interfaceFlags() const InterfaceFlags StagePrivate::interfaceFlags() const
{
InterfaceFlags result = announcedFlags();
result &= ~InterfaceFlags(OWN_IF_MASK);
result |= deducedFlags();
return result;
}
// return the interface flags that can be deduced from the interface
inline InterfaceFlags StagePrivate::deducedFlags() const
{ {
InterfaceFlags f; InterfaceFlags f;
if (starts_) f |= READS_START; if (starts()) f |= READS_START;
if (ends_) f |= READS_END; if (ends()) f |= READS_END;
if (prevEnds()) f |= WRITES_PREV_END; if (prevEnds()) f |= WRITES_PREV_END;
if (nextStarts()) f |= WRITES_NEXT_START; if (nextStarts()) f |= WRITES_NEXT_START;
return f; return f;
@ -62,7 +53,7 @@ inline bool implies(bool p, bool q) { return !p || q; }
void StagePrivate::validate() const { void StagePrivate::validate() const {
InitStageException errors; InitStageException errors;
InterfaceFlags f = announcedFlags(); InterfaceFlags f = interfaceFlags();
if (!implies(f & WRITES_NEXT_START, nextStarts())) if (!implies(f & WRITES_NEXT_START, nextStarts()))
errors.push_back(*me_, "sends forward, but next stage cannot receive"); errors.push_back(*me_, "sends forward, but next stage cannot receive");
@ -91,7 +82,7 @@ Stage::operator const StagePrivate *() const {
return pimpl(); return pimpl();
} }
void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) void Stage::reset()
{ {
auto impl = pimpl(); auto impl = pimpl();
if (impl->starts_) impl->starts_->clear(); if (impl->starts_) impl->starts_->clear();
@ -100,13 +91,17 @@ void Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
impl->next_starts_ = nullptr; impl->next_starts_ = nullptr;
} }
void Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
{
}
const std::string& Stage::name() const { const std::string& Stage::name() const {
return pimpl_->name_; return pimpl_->name_;
} }
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.deducedFlags(); InterfaceFlags f = stage.interfaceFlags();
bool own_if = f & own; bool own_if = f & own;
bool other_if = f & other; bool other_if = f & other;
bool reverse = own & INPUT_IF_MASK; bool reverse = own & INPUT_IF_MASK;
@ -160,9 +155,9 @@ const std::list<SubTrajectory> &ComputeBase::trajectories() const {
return pimpl()->trajectories_; return pimpl()->trajectories_;
} }
void ComputeBase::init(const planning_scene::PlanningSceneConstPtr &scene) { void ComputeBase::reset() {
pimpl()->trajectories_.clear(); pimpl()->trajectories_.clear();
Stage::init(scene); Stage::reset();
} }
@ -171,15 +166,6 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m
{ {
} }
InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
InterfaceFlags f;
if (dir & PropagatingEitherWay::FORWARD)
f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
if (dir & PropagatingEitherWay::BACKWARD)
f |= InterfaceFlags({READS_END, WRITES_PREV_END});
return f;
}
inline bool PropagatingEitherWayPrivate::hasStartState() const{ inline bool PropagatingEitherWayPrivate::hasStartState() const{
return next_start_state_ != starts_->end(); return next_start_state_ != starts_->end();
} }
@ -302,7 +288,11 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
ComputeBase::init(scene);
auto impl = pimpl(); auto impl = pimpl();
// after being connected, restrict actual interface directions
if (!impl->nextStarts()) { if (!impl->nextStarts()) {
impl->starts_.reset(); impl->starts_.reset();
impl->next_start_state_ = Interface::iterator(); impl->next_start_state_ = Interface::iterator();
@ -311,7 +301,8 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce
impl->ends_.reset(); impl->ends_.reset();
impl->next_end_state_ = Interface::iterator(); impl->next_end_state_ = Interface::iterator();
} }
ComputeBase::init(scene); if (!impl->isConnected())
throw InitStageException(*this, "can neither send forwards nor backwards");
} }
void PropagatingEitherWay::sendForward(const InterfaceState& from, void PropagatingEitherWay::sendForward(const InterfaceState& from,
@ -383,10 +374,6 @@ GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
: ComputeBasePrivate(me, name) : ComputeBasePrivate(me, name)
{} {}
InterfaceFlags GeneratorPrivate::announcedFlags() const {
return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END});
}
bool GeneratorPrivate::canCompute() const { bool GeneratorPrivate::canCompute() const {
return static_cast<Generator*>(me_)->canCompute(); return static_cast<Generator*>(me_)->canCompute();
} }
@ -425,10 +412,6 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin()); it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
} }
InterfaceFlags ConnectingPrivate::announcedFlags() const {
return InterfaceFlags({READS_START, READS_END});
}
void ConnectingPrivate::newStartState(const Interface::iterator& it) void ConnectingPrivate::newStartState(const Interface::iterator& it)
{ {
// TODO: need to handle the pairs iterator // TODO: need to handle the pairs iterator

View File

@ -36,8 +36,6 @@ public:
StagePrivate(Stage* me, const std::string& name); StagePrivate(Stage* me, const std::string& name);
InterfaceFlags interfaceFlags() const; InterfaceFlags interfaceFlags() const;
InterfaceFlags deducedFlags() const;
virtual InterfaceFlags announcedFlags() const = 0;
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual bool compute() = 0; virtual bool compute() = 0;
@ -108,8 +106,7 @@ public:
PropagatingEitherWay::Direction dir; PropagatingEitherWay::Direction dir;
inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir,
const std::string &name); const std::string &name);
InterfaceFlags announcedFlags() const override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;
@ -145,7 +142,6 @@ public:
class GeneratorPrivate : public ComputeBasePrivate { class GeneratorPrivate : public ComputeBasePrivate {
public: public:
inline GeneratorPrivate(Generator *me, const std::string &name); inline GeneratorPrivate(Generator *me, const std::string &name);
InterfaceFlags announcedFlags() const override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;
@ -157,7 +153,6 @@ class ConnectingPrivate : public ComputeBasePrivate {
public: public:
inline ConnectingPrivate(Connecting *me, const std::string &name); inline ConnectingPrivate(Connecting *me, const std::string &name);
InterfaceFlags announcedFlags() const override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;

View File

@ -118,6 +118,7 @@ bool Task::plan(){
auto impl = pimpl(); auto impl = pimpl();
setSolutionCallback(NewSolutionPublisher()); setSolutionCallback(NewSolutionPublisher());
reset();
impl->initScene(); impl->initScene();
init(impl->scene_); init(impl->scene_);