setup SerialContainer interface

This commit is contained in:
Robert Haschke 2017-10-19 20:49:58 +02:00
parent bc5c76578e
commit 68ff79f464
5 changed files with 48 additions and 11 deletions

View File

@ -36,8 +36,12 @@ public:
// TODO turn this into priority queue // TODO turn this into priority queue
typedef std::deque<SolutionBase*> Solutions; typedef std::deque<SolutionBase*> Solutions;
/// create an InterfaceState from a planning scene
InterfaceState(const planning_scene::PlanningSceneConstPtr& ps); 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 planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; } inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; } inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
@ -71,6 +75,9 @@ public:
// and finally run the notify callback // and finally run the notify callback
container_type::iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); 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::value_type;
using container_type::reference; using container_type::reference;
using container_type::const_reference; using container_type::const_reference;
@ -127,16 +134,15 @@ protected:
: creator_(creator), cost_(cost) : 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: private:
// back-pointer to creating stage, allows to access sub-solutions // back-pointer to creating stage, allows to access sub-solutions
StagePrivate *creator_; StagePrivate *creator_;
// associated cost // associated cost
double cost_; double cost_;
// begin and end InterfaceState of this solution/trajectory
const InterfaceState* start_ = nullptr;
const InterfaceState* end_ = nullptr;
}; };

View File

@ -95,9 +95,7 @@ void ContainerBase::clear()
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
{ {
// TODO rhaschke: define notify functions // these lists don't need a notify function, connections are handled by onNewSolution()
starts_.reset(new Interface(Interface::NotifyFunction()));
ends_.reset(new Interface(Interface::NotifyFunction()));
pending_backward_.reset(new Interface(Interface::NotifyFunction())); pending_backward_.reset(new Interface(Interface::NotifyFunction()));
pending_forward_.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()) if (impl->children().empty())
return false; 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 ***/ /*** connect children ***/
// first stage sends backward to pending_backward_ // first stage sends backward to pending_backward_
auto cur = impl->children().begin();
(*cur)->pimpl()->setPrevEnds(impl->pending_backward_.get()); (*cur)->pimpl()->setPrevEnds(impl->pending_backward_.get());
// last stage sends forward to pending_forward_ // last stage sends forward to pending_forward_
auto last = --impl->children().end();
(*last)->pimpl()->setNextStarts(impl->pending_forward_.get()); (*last)->pimpl()->setNextStarts(impl->pending_forward_.get());
auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage

View File

@ -378,6 +378,9 @@ PIMPL_FUNCTIONS(Generator)
void Generator::spawn(InterfaceState&& state, double cost) void Generator::spawn(InterfaceState&& state, double cost)
{ {
std::cout << "spawning state forwards and backwards" << std::endl; std::cout << "spawning state forwards and backwards" << std::endl;
assert(state.incomingTrajectories().empty() &&
state.outgoingTrajectories().empty());
auto impl = pimpl(); auto impl = pimpl();
// empty trajectory ref -> this node only produces states // empty trajectory ref -> this node only produces states
robot_trajectory::RobotTrajectoryPtr dummy; robot_trajectory::RobotTrajectoryPtr dummy;

View File

@ -51,7 +51,8 @@ public:
inline Interface* nextStarts() const { return next_starts_; } inline Interface* nextStarts() const { return next_starts_; }
inline bool isConnected() const { return prev_ends_ || 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; bool validate() const;
inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) { inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) {

View File

@ -7,6 +7,11 @@ InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
{ {
} }
InterfaceState::InterfaceState(const InterfaceState &existing)
: scene_(existing.scene())
{
}
Interface::Interface(const Interface::NotifyFunction &notify) Interface::Interface(const Interface::NotifyFunction &notify)
: notify_(notify) : notify_(notify)
{} {}
@ -28,6 +33,14 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
return back; 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) { void SolutionBase::setCost(double cost) {
cost_ = cost; cost_ = cost;