From 68ff79f464d5c208094609188e6262a82d1385ba Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 19 Oct 2017 20:49:58 +0200 Subject: [PATCH] setup SerialContainer interface --- include/moveit_task_constructor/storage.h | 16 ++++++++++----- src/container.cpp | 24 ++++++++++++++++++----- src/stage.cpp | 3 +++ src/stage_p.h | 3 ++- src/storage.cpp | 13 ++++++++++++ 5 files changed, 48 insertions(+), 11 deletions(-) diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index fc9f67f0..84c45bf9 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -36,8 +36,12 @@ public: // TODO turn this into priority queue typedef std::deque Solutions; + /// create an InterfaceState from a planning scene InterfaceState(const planning_scene::PlanningSceneConstPtr& ps); + /// copy an existing InterfaceState, but not including incoming/outgoing trajectories + InterfaceState(const InterfaceState& existing); + inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; } inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; } inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; } @@ -71,6 +75,9 @@ public: // and finally run the notify callback container_type::iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); + // clone an existing InterfaceState, but without its incoming/outgoing connections + container_type::iterator clone(const InterfaceState &state); + using container_type::value_type; using container_type::reference; using container_type::const_reference; @@ -127,16 +134,15 @@ protected: : creator_(creator), cost_(cost) {} -public: // TODO: make private - // begin and end InterfaceState for this solution/trajectory - const InterfaceState* start_ = nullptr; - const InterfaceState* end_ = nullptr; - private: // back-pointer to creating stage, allows to access sub-solutions StagePrivate *creator_; // associated cost double cost_; + + // begin and end InterfaceState of this solution/trajectory + const InterfaceState* start_ = nullptr; + const InterfaceState* end_ = nullptr; }; diff --git a/src/container.cpp b/src/container.cpp index c50136bf..11506674 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -95,9 +95,7 @@ void ContainerBase::clear() SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) : ContainerBasePrivate(me, name) { - // TODO rhaschke: define notify functions - starts_.reset(new Interface(Interface::NotifyFunction())); - ends_.reset(new Interface(Interface::NotifyFunction())); + // these lists don't need a notify function, connections are handled by onNewSolution() pending_backward_.reset(new Interface(Interface::NotifyFunction())); pending_forward_.reset(new Interface(Interface::NotifyFunction())); } @@ -253,13 +251,29 @@ bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) if (impl->children().empty()) return false; + // initialize starts_ and ends_ interfaces + auto cur = impl->children().begin(); + StagePrivate* child_impl = **cur; + if (child_impl->starts()) + impl->starts_.reset(new Interface([impl, child_impl](const Interface::iterator& internal){ + // new state in our starts_ interface is copied to first child, remembering the link + auto it = child_impl->starts()->clone(*internal); + impl->internal_to_my_starts_.insert(std::make_pair(&*it, &*internal)); + })); + + auto last = --impl->children().end(); + if ((*cur)->pimpl()->ends()) + 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 + auto it = child_impl->ends()->clone(*internal); + impl->internal_to_my_ends_.insert(std::make_pair(&*it, &*internal)); + })); + /*** connect children ***/ // first stage sends backward to pending_backward_ - auto cur = impl->children().begin(); (*cur)->pimpl()->setPrevEnds(impl->pending_backward_.get()); // last stage sends forward to pending_forward_ - auto last = --impl->children().end(); (*last)->pimpl()->setNextStarts(impl->pending_forward_.get()); auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage diff --git a/src/stage.cpp b/src/stage.cpp index c4172e6d..22c4b05f 100644 --- a/src/stage.cpp +++ b/src/stage.cpp @@ -378,6 +378,9 @@ PIMPL_FUNCTIONS(Generator) void Generator::spawn(InterfaceState&& state, double cost) { std::cout << "spawning state forwards and backwards" << std::endl; + assert(state.incomingTrajectories().empty() && + state.outgoingTrajectories().empty()); + auto impl = pimpl(); // empty trajectory ref -> this node only produces states robot_trajectory::RobotTrajectoryPtr dummy; diff --git a/src/stage_p.h b/src/stage_p.h index 956088cb..a0d2994f 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -51,7 +51,8 @@ public: inline Interface* nextStarts() const { return next_starts_; } inline bool isConnected() const { return prev_ends_ || next_starts_; } - // validate that sendForward() and sendBackward() will succeed + /// validate that sendForward() and sendBackward() will succeed + /// should be only called by containers' init() method bool validate() const; inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) { diff --git a/src/storage.cpp b/src/storage.cpp index a013747a..4bc5e11e 100644 --- a/src/storage.cpp +++ b/src/storage.cpp @@ -7,6 +7,11 @@ InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps) { } +InterfaceState::InterfaceState(const InterfaceState &existing) + : scene_(existing.scene()) +{ +} + Interface::Interface(const Interface::NotifyFunction ¬ify) : notify_(notify) {} @@ -28,6 +33,14 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin return back; } +Interface::iterator Interface::clone(const InterfaceState &state) +{ + emplace_back(InterfaceState(state)); + iterator back = --end(); + if (notify_) notify_(back); + return back; +} + void SolutionBase::setCost(double cost) { cost_ = cost;