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
|
// 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;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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)
|
Interface::Interface(const Interface::NotifyFunction ¬ify)
|
||||||
: 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;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user