mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
setup SerialContainer interface
This commit is contained in:
parent
bc5c76578e
commit
68ff79f464
@ -36,8 +36,12 @@ public:
|
||||
// TODO turn this into priority queue
|
||||
typedef std::deque<SolutionBase*> 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;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user