mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cleanup container
- removed friends + mutable - simplified SerialContainer::canInsert()
This commit is contained in:
parent
dc9f553ab0
commit
3cc112fe2c
@ -30,8 +30,6 @@ MOVEIT_CLASS_FORWARD(SubTask)
|
||||
* A start or goal state for planning is essentially defined by the state of a planning scene.
|
||||
*/
|
||||
class InterfaceState {
|
||||
friend class SubTrajectory;
|
||||
|
||||
public:
|
||||
typedef std::deque<SubTrajectory*> SubTrajectoryList;
|
||||
|
||||
@ -42,12 +40,15 @@ public:
|
||||
inline const SubTrajectoryList& incomingTrajectories() const { return incoming_trajectories_; }
|
||||
inline const SubTrajectoryList& outgoingTrajectories() const { return outgoing_trajectories_; }
|
||||
|
||||
inline void addIncoming(SubTrajectory* t) { incoming_trajectories_.push_back(t); }
|
||||
inline void addOutgoing(SubTrajectory* t) { outgoing_trajectories_.push_back(t); }
|
||||
|
||||
public:
|
||||
planning_scene::PlanningSceneConstPtr state;
|
||||
|
||||
private:
|
||||
mutable SubTrajectoryList incoming_trajectories_;
|
||||
mutable SubTrajectoryList outgoing_trajectories_;
|
||||
SubTrajectoryList incoming_trajectories_;
|
||||
SubTrajectoryList outgoing_trajectories_;
|
||||
};
|
||||
|
||||
|
||||
@ -105,13 +106,13 @@ struct SubTrajectory {
|
||||
inline void setStartState(const InterfaceState& state){
|
||||
assert(begin == NULL);
|
||||
begin= &state;
|
||||
state.outgoing_trajectories_.push_back(this);
|
||||
const_cast<InterfaceState&>(state).addOutgoing(this);
|
||||
}
|
||||
|
||||
inline void setEndState(const InterfaceState& state){
|
||||
assert(end == NULL);
|
||||
end= &state;
|
||||
state.incoming_trajectories_.push_back(this);
|
||||
const_cast<InterfaceState&>(state).addIncoming(this);
|
||||
}
|
||||
|
||||
// TODO: trajectories could have a name, e.g. a generator could name its solutions
|
||||
|
||||
@ -21,8 +21,8 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
|
||||
|
||||
inline bool ContainerBasePrivate::canInsert(const SubTask &stage) const {
|
||||
const SubTaskPrivate* impl = stage.pimpl();
|
||||
return impl->parent_ == nullptr // re-parenting is not supported
|
||||
&& impl->trajectories_.empty(); // existing trajectories would become invalid
|
||||
return impl->parent() == nullptr // re-parenting is not supported
|
||||
&& impl->trajectories().empty(); // existing trajectories would become invalid
|
||||
}
|
||||
|
||||
bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, int depth) const {
|
||||
@ -39,9 +39,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
|
||||
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask,
|
||||
ContainerBasePrivate::const_iterator pos) {
|
||||
SubTaskPrivate *impl = subtask->pimpl();
|
||||
impl->parent_ = this;
|
||||
impl->it_ = children_.insert(pos, std::move(subtask));
|
||||
return impl->it_;
|
||||
ContainerBasePrivate::iterator it = children_.insert(pos, std::move(subtask));
|
||||
impl->setHierarchy(this, it);
|
||||
return it;
|
||||
}
|
||||
|
||||
|
||||
@ -53,8 +53,7 @@ PIMPL_FUNCTIONS(ContainerBase)
|
||||
|
||||
void ContainerBase::clear()
|
||||
{
|
||||
auto impl = pimpl();
|
||||
impl->clear();
|
||||
pimpl()->children_.clear();
|
||||
}
|
||||
|
||||
bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
@ -66,22 +65,26 @@ bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
|
||||
bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
|
||||
{
|
||||
auto impl = pimpl();
|
||||
return impl->traverseStages(processor, 0);
|
||||
pimpl()->traverseStages(processor, 0);
|
||||
}
|
||||
|
||||
bool ContainerBase::canCompute() const
|
||||
{
|
||||
auto impl = pimpl();
|
||||
return impl->canCompute();
|
||||
pimpl()->canCompute();
|
||||
}
|
||||
|
||||
bool ContainerBase::compute() {
|
||||
auto impl = pimpl();
|
||||
return impl->compute();
|
||||
pimpl()->compute();
|
||||
}
|
||||
|
||||
|
||||
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
|
||||
: ContainerBasePrivate(me, name)
|
||||
{
|
||||
starts_.reset(new Interface(Interface::NotifyFunction()));
|
||||
ends_.reset(new Interface(Interface::NotifyFunction()));
|
||||
}
|
||||
|
||||
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
||||
InterfaceFlags f;
|
||||
if (children().empty()) return f;
|
||||
@ -90,31 +93,9 @@ SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
||||
return f;
|
||||
}
|
||||
|
||||
inline bool isConnectable(int prev_flags, int next_flags) {
|
||||
return ((prev_flags & SubTaskPrivate::WRITES_NEXT_START) && (next_flags & SubTaskPrivate::READS_START)) ||
|
||||
((prev_flags & SubTaskPrivate::READS_END) && (next_flags & SubTaskPrivate::WRITES_PREV_END));
|
||||
}
|
||||
inline bool bothWrite(SubTaskPrivate::InterfaceFlags prev_flags, SubTaskPrivate::InterfaceFlags next_flags) {
|
||||
return (prev_flags.testFlag(SubTaskPrivate::WRITES_NEXT_START) && !next_flags.testFlag(SubTaskPrivate::READS_START)) &&
|
||||
(next_flags.testFlag(SubTaskPrivate::WRITES_PREV_END) && !prev_flags.testFlag(SubTaskPrivate::READS_END));
|
||||
}
|
||||
|
||||
inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBasePrivate::const_iterator before) const {
|
||||
if (!ContainerBasePrivate::canInsert(stage))
|
||||
return false;
|
||||
|
||||
// check connectedness
|
||||
bool at_end = (before == children().end());
|
||||
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl();
|
||||
InterfaceFlags cur_flags = stage.pimpl()->announcedFlags();
|
||||
InterfaceFlags next_flags = next->deducedFlags();
|
||||
InterfaceFlags prev_flags = prev(before)->deducedFlags();
|
||||
|
||||
// Do a simple check here only. A full connectivity check requires the full pipeline to be setup
|
||||
// Thus, here we reject when trying to connect to writers with each other
|
||||
if (bothWrite(prev_flags, cur_flags) || bothWrite(cur_flags, next_flags))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -127,66 +108,42 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
|
||||
SubTaskPrivate *cur = stage->pimpl();
|
||||
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
|
||||
if (children().empty()) { // first child inserted
|
||||
setPrevEnds(cur, this->starts_);
|
||||
setNextStarts(cur, this->ends_);
|
||||
cur->setPrevEnds(this->starts());
|
||||
cur->setNextStarts(this->ends());
|
||||
} else if (at_begin) {
|
||||
SubTaskPrivate *next = (*before)->pimpl();
|
||||
setPrevEnds(cur, this->starts_);
|
||||
setNextStarts(cur, next->starts_);
|
||||
setPrevEnds(next, cur->ends_);
|
||||
cur->setPrevEnds(this->starts());
|
||||
cur->setNextStarts(next->starts());
|
||||
next->setPrevEnds(cur->ends());
|
||||
} else if (at_end) {
|
||||
const SubTaskPrivate *prev = this->prev(before);
|
||||
setNextStarts(prev, cur->starts_);
|
||||
setPrevEnds(cur, prev->ends_);
|
||||
setNextStarts(cur, this->ends_);
|
||||
SubTaskPrivate *prev = (*this->prev(before))->pimpl();
|
||||
prev->setNextStarts(cur->starts());
|
||||
cur->setPrevEnds(prev->ends());
|
||||
cur->setNextStarts(this->ends());
|
||||
} else {
|
||||
const SubTaskPrivate *prev = this->prev(before);
|
||||
SubTaskPrivate *prev = (*this->prev(before))->pimpl();
|
||||
SubTaskPrivate *next = (*before)->pimpl();
|
||||
setNextStarts(prev, cur->starts_);
|
||||
setPrevEnds(cur, prev->ends_);
|
||||
setNextStarts(cur, next->starts_);
|
||||
setPrevEnds(next, cur->ends_);
|
||||
prev->setNextStarts(cur->starts());
|
||||
cur->setPrevEnds(prev->ends());
|
||||
cur->setNextStarts(next->starts());
|
||||
next->setPrevEnds(cur->ends());
|
||||
}
|
||||
|
||||
iterator it = ContainerBasePrivate::insert(std::move(stage), before);
|
||||
}
|
||||
|
||||
inline const SubTaskPrivate* SerialContainerPrivate::prev(const_iterator it) const
|
||||
inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const
|
||||
{
|
||||
#ifndef NDEBUG
|
||||
if (it != children().end()) {
|
||||
SubTaskPrivate* child = (*it)->pimpl();
|
||||
assert(parent(child) == this);
|
||||
assert(this->it(child) == it);
|
||||
}
|
||||
#endif
|
||||
if (it == children().begin()) return this;
|
||||
return (*--it)->pimpl();
|
||||
assert(it != children().cbegin());
|
||||
return --it;
|
||||
}
|
||||
|
||||
inline const SubTaskPrivate* SerialContainerPrivate::next(const_iterator it) const
|
||||
inline ContainerBasePrivate::const_iterator SerialContainerPrivate::next(const_iterator it) const
|
||||
{
|
||||
#ifndef NDEBUG
|
||||
assert(it != children().end());
|
||||
SubTaskPrivate* child = (*it)->pimpl();
|
||||
assert(parent(child) == this);
|
||||
assert(this->it(child) == it);
|
||||
#endif
|
||||
if (it == --children().end()) return this;
|
||||
return (*++it)->pimpl();
|
||||
assert(it != children().cend());
|
||||
return ++it;
|
||||
}
|
||||
|
||||
const SubTaskPrivate *SerialContainerPrivate::prev(const SubTaskPrivate *child) const
|
||||
{
|
||||
return prev(it(child));
|
||||
}
|
||||
|
||||
const SubTaskPrivate *SerialContainerPrivate::next(const SubTaskPrivate *child) const
|
||||
{
|
||||
return next(it(child));
|
||||
}
|
||||
|
||||
|
||||
SerialContainer::SerialContainer(SerialContainerPrivate *impl)
|
||||
: ContainerBase(impl)
|
||||
{}
|
||||
|
||||
@ -8,7 +8,6 @@ namespace moveit { namespace task_constructor {
|
||||
class ContainerBasePrivate : public SubTaskPrivate
|
||||
{
|
||||
friend class ContainerBase;
|
||||
friend class BaseTest; // allow access for unit tests
|
||||
|
||||
public:
|
||||
typedef ContainerBase::value_type value_type;
|
||||
@ -21,7 +20,6 @@ public:
|
||||
|
||||
bool canInsert(const SubTask& stage) const;
|
||||
virtual iterator insert(value_type &&subtask, const_iterator pos);
|
||||
inline void clear() { children_.clear(); }
|
||||
|
||||
bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const;
|
||||
|
||||
@ -29,15 +27,6 @@ protected:
|
||||
ContainerBasePrivate(ContainerBase *me, const std::string &name)
|
||||
: SubTaskPrivate(me, name)
|
||||
{}
|
||||
inline const ContainerBasePrivate* parent(const SubTaskPrivate *child) const { return child->parent_; }
|
||||
inline iterator it(const SubTaskPrivate *child) const { return child->it_; }
|
||||
|
||||
inline void setPrevEnds(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
|
||||
child->prev_ends_ = interface.get();
|
||||
}
|
||||
inline void setNextStarts(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
|
||||
child->next_starts_ = interface.get();
|
||||
}
|
||||
|
||||
private:
|
||||
container_type children_;
|
||||
@ -46,25 +35,18 @@ private:
|
||||
|
||||
class SerialContainerPrivate : public ContainerBasePrivate {
|
||||
public:
|
||||
SerialContainerPrivate(SerialContainer* me, const std::string &name)
|
||||
: ContainerBasePrivate(me, name)
|
||||
{
|
||||
starts_.reset(new Interface(Interface::NotifyFunction()));
|
||||
ends_.reset(new Interface(Interface::NotifyFunction()));
|
||||
}
|
||||
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
||||
|
||||
InterfaceFlags announcedFlags() const override;
|
||||
bool canInsert(const SubTask& stage, const_iterator before) const;
|
||||
inline bool canInsert(const SubTask& stage, const_iterator before) const;
|
||||
virtual iterator insert(value_type &&stage, const_iterator before) override;
|
||||
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
|
||||
inline const SubTaskPrivate *prev(const_iterator it) const;
|
||||
inline const SubTaskPrivate *next(const_iterator it) const;
|
||||
|
||||
const SubTaskPrivate *prev(const SubTaskPrivate *child) const;
|
||||
const SubTaskPrivate *next(const SubTaskPrivate *child) const;
|
||||
private:
|
||||
inline const_iterator prev(const_iterator it) const;
|
||||
inline const_iterator next(const_iterator it) const;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -15,8 +15,6 @@ namespace moveit { namespace task_constructor {
|
||||
class ContainerBasePrivate;
|
||||
class SubTaskPrivate {
|
||||
friend class SubTask;
|
||||
friend class BaseTest; // allow access for unit tests
|
||||
friend class ContainerBasePrivate; // allow to set parent_ and it_
|
||||
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||
|
||||
public:
|
||||
@ -45,29 +43,39 @@ public:
|
||||
virtual bool canCompute() const = 0;
|
||||
virtual bool compute() = 0;
|
||||
|
||||
public:
|
||||
inline ContainerBasePrivate* parent() const { return parent_; }
|
||||
inline container_type::iterator it() const { return it_; }
|
||||
inline Interface* starts() const { return starts_.get(); }
|
||||
inline Interface* ends() const { return ends_.get(); }
|
||||
inline Interface* prevEnds() const { return prev_ends_; }
|
||||
inline Interface* nextStarts() const { return next_starts_; }
|
||||
inline const std::list<SubTrajectory> trajectories() const { return trajectories_; }
|
||||
|
||||
inline bool isConnected() const { return prev_ends_ || next_starts_; }
|
||||
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
||||
|
||||
inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) {
|
||||
parent_ = parent;
|
||||
it_ = it;
|
||||
}
|
||||
inline void setPrevEnds(Interface * prev_ends) { prev_ends_ = prev_ends; }
|
||||
inline void setNextStarts(Interface * next_starts) { next_starts_ = next_starts; }
|
||||
|
||||
protected:
|
||||
SubTask* const me_; // associated/owning SubTask instance
|
||||
const std::string name_;
|
||||
|
||||
InterfacePtr starts_;
|
||||
InterfacePtr ends_;
|
||||
|
||||
inline ContainerBasePrivate* parent() const { return parent_; }
|
||||
inline bool isConnected() const { return prev_ends_ || next_starts_; }
|
||||
inline Interface* prevEnds() const { return prev_ends_; }
|
||||
inline Interface* nextStarts() const { return next_starts_; }
|
||||
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
||||
|
||||
protected:
|
||||
std::list<SubTrajectory> trajectories_;
|
||||
|
||||
private:
|
||||
// !! items accessed only by ContainerBasePrivate to maintain hierarchy !!
|
||||
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
|
||||
ContainerBasePrivate* parent_; // owning parent
|
||||
container_type::iterator it_; // iterator into parent's children_ list referring to this
|
||||
// caching the pointers to the ends_ / starts_ interface of previous / next stage
|
||||
mutable Interface *prev_ends_; // interface to be used for sendBackward()
|
||||
mutable Interface *next_starts_; // interface to be use for sendForward()
|
||||
|
||||
Interface *prev_ends_; // interface to be used for sendBackward()
|
||||
Interface *next_starts_; // interface to be used for sendForward()
|
||||
};
|
||||
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||
|
||||
|
||||
@ -44,9 +44,6 @@ protected:
|
||||
void SetUp() {}
|
||||
void TearDown() {}
|
||||
|
||||
// accessors for private elements of SubTaskPrivate
|
||||
inline const SubTaskPrivate* parent(const SubTaskPrivate* p) const { return p->parent_; }
|
||||
|
||||
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<SubTaskPrivate*> &expected) {
|
||||
size_t num = container->children().size();
|
||||
ASSERT_TRUE(num == expected.size()) << "different list lengths";
|
||||
@ -61,24 +58,15 @@ protected:
|
||||
PRINTF(" *** parent: %p ***\n", container);
|
||||
|
||||
// validate order
|
||||
const SubTaskPrivate* predeccessor = container;
|
||||
const SubTaskPrivate* successor = container;
|
||||
size_t pos = 0;
|
||||
auto exp_it = expected.begin();
|
||||
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) {
|
||||
SubTaskPrivate *child = (*it)->pimpl();
|
||||
EXPECT_EQ(child, *exp_it) << "wrong order";
|
||||
EXPECT_EQ(child->parent_, container) << "wrong parent";
|
||||
EXPECT_EQ(child->parent(), container) << "wrong parent";
|
||||
EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution";
|
||||
EXPECT_EQ(it, container->position(pos-num-1)) << "bad backward position resolution";
|
||||
EXPECT_EQ(container->prev(child), predeccessor) << "wrong link to predeccessor for child " << child;
|
||||
if (successor != container)
|
||||
EXPECT_EQ(child, successor) << "wrong link to successor for child " << predeccessor;
|
||||
// store predeccessor and successor for next round
|
||||
predeccessor = child;
|
||||
successor = container->next(child);
|
||||
}
|
||||
EXPECT_EQ(successor, container);
|
||||
}
|
||||
};
|
||||
|
||||
@ -95,39 +83,30 @@ TEST_F(BaseTest, interfaceFlags) {
|
||||
|
||||
TEST_F(BaseTest, serialContainer) {
|
||||
SerialContainer c("serial");
|
||||
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(c.pimpl());
|
||||
SerialContainerPrivate *cp = c.pimpl();
|
||||
|
||||
EXPECT_TRUE(bool(cp->starts_));
|
||||
EXPECT_TRUE(bool(cp->ends_));
|
||||
EXPECT_EQ(parent(cp), nullptr);
|
||||
EXPECT_EQ(parent(cp), nullptr);
|
||||
EXPECT_TRUE(bool(cp->starts()));
|
||||
EXPECT_TRUE(bool(cp->ends()));
|
||||
EXPECT_EQ(cp->parent(), nullptr);
|
||||
VALIDATE();
|
||||
|
||||
/***** inserting first stage *****/
|
||||
auto g = std::make_unique<TestGenerator>();
|
||||
GeneratorPrivate *gp = static_cast<GeneratorPrivate*>(g->pimpl());
|
||||
GeneratorPrivate *gp = g->pimpl();
|
||||
ASSERT_TRUE(c.insert(std::move(g)));
|
||||
EXPECT_FALSE(g); // ownership transferred to container
|
||||
VALIDATE(gp);
|
||||
|
||||
// inserting another generator should fail
|
||||
g = std::make_unique<TestGenerator>();
|
||||
EXPECT_FALSE(c.insert(std::move(g)));
|
||||
|
||||
/***** inserting second stage *****/
|
||||
auto f = std::make_unique<TestPropagatingForward>();
|
||||
PropagatingForwardPrivate *fp = static_cast<PropagatingForwardPrivate*>(f->pimpl());
|
||||
PropagatingForwardPrivate *fp = f->pimpl();
|
||||
ASSERT_TRUE(c.insert(std::move(f)));
|
||||
EXPECT_FALSE(f); // ownership transferred to container
|
||||
VALIDATE(gp, fp);
|
||||
|
||||
/***** inserting third stage *****/
|
||||
auto f2 = std::make_unique<TestPropagatingForward>();
|
||||
PropagatingForwardPrivate *fp2 = static_cast<PropagatingForwardPrivate*>(f2->pimpl());
|
||||
EXPECT_FALSE(c.insert(std::move(f2), 0)); // should fail at first position
|
||||
|
||||
// insert @2nd position
|
||||
f2 = std::make_unique<TestPropagatingForward>();
|
||||
PropagatingForwardPrivate *fp2 = f2->pimpl();
|
||||
ASSERT_TRUE(c.insert(std::move(f2), 1));
|
||||
EXPECT_FALSE(f2); // ownership transferred to container
|
||||
VALIDATE(gp, fp2, fp);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user