mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
separate reset() + init()
deducedFlags() -> interfaceFlags() removed announcedFlags()
This commit is contained in:
parent
43bcf99222
commit
f103341356
@ -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;
|
||||||
|
|||||||
@ -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:
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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_; }
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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_);
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user