From 6167f728edc9d65c1ffe7abb0da587efb81b6aa4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 16 Oct 2017 16:21:53 +0200 Subject: [PATCH] wip --- include/moveit_task_constructor/container.h | 40 +++- include/moveit_task_constructor/debug.h | 2 +- include/moveit_task_constructor/stage.h | 47 +++- include/moveit_task_constructor/storage.h | 80 ++++--- include/moveit_task_constructor/task.h | 3 +- src/container.cpp | 241 ++++++++++++++------ src/container_p.h | 64 +++++- src/debug.cpp | 10 +- src/stage.cpp | 192 ++++++++++------ src/stage_p.h | 27 ++- src/stages/current_state.cpp | 1 + src/stages/generate_grasp_pose.cpp | 1 + src/stages/gripper.cpp | 1 + src/stages/move.cpp | 1 + src/storage.cpp | 7 +- src/task.cpp | 43 +--- 16 files changed, 520 insertions(+), 240 deletions(-) diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index c6e90735..d93b4568 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -9,20 +9,22 @@ class ContainerBase : public Stage { public: PRIVATE_CLASS(ContainerBase) - typedef Stage::pointer value_type; + + size_t numChildren() const; typedef std::function StageCallback; - typedef std::function&)> SolutionCallback; + /// traverse direct children of this container, calling the callback for each of them + bool traverseChildren(const StageCallback &processor) const; + /// traverse all children of this container recursively + bool traverseRecursively(const StageCallback &processor) const; - virtual bool canInsert(const value_type& stage, int before = -1) const = 0; - virtual bool insert(value_type&& stage, int before = -1) = 0; + virtual bool insert(Stage::pointer&& stage, int before = -1); virtual void clear(); - bool init(const planning_scene::PlanningSceneConstPtr &scene) override; - bool canCompute() const; - bool compute(); + virtual bool canCompute() const = 0; + virtual bool compute() = 0; - bool traverseStages(const StageCallback &processor) const; + size_t numSolutions() const = 0; protected: ContainerBase(ContainerBasePrivate* impl); @@ -35,8 +37,26 @@ public: PRIVATE_CLASS(SerialContainer) SerialContainer(const std::string& name); - bool canInsert(const value_type& stage, int before = -1) const override; - bool insert(value_type&& stage, int before = -1) override; + bool init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool canCompute() const override; + bool compute() override; + + size_t numSolutions() const override; + + /// function type used for traversing solutions + /// For each sub solution (current), the trace from the start as well as the + /// accumulated cost of all solutions in the trace are provided. + /// Return true, if traversal should continue, otherwise false. + typedef std::function& trace, + double trace_accumulated_cost)> SolutionCallback; + +protected: + /// traverse all solutions, starting at start and call the callback for each subsolution + /// The return value is always false, indicating that the traversal eventually stopped. + template + bool traverse(const SolutionBase &start, const SolutionCallback &cb, + std::vector &trace, double trace_cost = 0); protected: SerialContainer(SerialContainerPrivate* impl); diff --git a/include/moveit_task_constructor/debug.h b/include/moveit_task_constructor/debug.h index c99abba5..86d6f1f7 100644 --- a/include/moveit_task_constructor/debug.h +++ b/include/moveit_task_constructor/debug.h @@ -12,7 +12,7 @@ MOVEIT_CLASS_FORWARD(Task) MOVEIT_CLASS_FORWARD(SubTrajectory) bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, - const std::vector& solution, bool wait); + const std::vector &solution, bool wait); void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true); diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index 41d4e8d5..f1a95f2f 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -5,6 +5,7 @@ #include "utils.h" #include +#include #include #include @@ -29,11 +30,26 @@ namespace moveit { namespace task_constructor { MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Stage) -MOVEIT_CLASS_FORWARD(SubTrajectory) class InterfaceState; typedef std::pair InterfaceStatePair; +// SubTrajectory connects interface states of ComputeStages +class SubTrajectory : public SolutionBase { +public: + explicit SubTrajectory(StagePrivate* creator, const robot_trajectory::RobotTrajectoryPtr& traj, double cost); + + robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } + const std::string& name() const { return name_; } + void setName(const std::string& name) { name_ = name; } + +private: + const robot_trajectory::RobotTrajectoryPtr trajectory_; + // trajectories could have a name, e.g. a generator could name its solutions + std::string name_; +}; + + class StagePrivate; class Stage { friend std::ostream& operator<<(std::ostream &os, const Stage& stage); @@ -41,26 +57,42 @@ class Stage { public: PRIVATE_CLASS(Stage) typedef std::unique_ptr pointer; - ~Stage(); /// initialize stage once before planning virtual bool init(const planning_scene::PlanningSceneConstPtr& scene); - const std::string& getName() const; + + const std::string& name() const; + virtual size_t numSolutions() const = 0; protected: /// Stage can only be instantiated through derived classes Stage(StagePrivate *impl); protected: - // TODO: use unique_ptr and get rid of destructor StagePrivate* const pimpl_; // constness guarantees one initial write }; std::ostream& operator<<(std::ostream &os, const Stage& stage); +class ComputeBasePrivate; +class ComputeBase : public Stage { +public: + PRIVATE_CLASS(ComputeBase) + virtual size_t numSolutions() const override; + +protected: + /// ComputeBase can only be instantiated by derived classes in stage.cpp + ComputeBase(ComputeBasePrivate* impl); + + // TODO: Do we really need/want to expose the trajectories? + const std::list& trajectories() const; + SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost); +}; + + class PropagatingEitherWayPrivate; -class PropagatingEitherWay : public Stage { +class PropagatingEitherWay : public ComputeBase { public: PRIVATE_CLASS(PropagatingEitherWay) PropagatingEitherWay(const std::string& name); @@ -68,6 +100,7 @@ public: enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD}; void restrictDirection(Direction dir); + virtual bool init(const planning_scene::PlanningSceneConstPtr &scene) override; virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeBackward(const InterfaceState& to) = 0; @@ -114,7 +147,7 @@ private: class GeneratorPrivate; -class Generator : public Stage { +class Generator : public ComputeBase { public: PRIVATE_CLASS(Generator) Generator(const std::string& name); @@ -126,7 +159,7 @@ public: class ConnectingPrivate; -class Connecting : public Stage { +class Connecting : public ComputeBase { public: PRIVATE_CLASS(Connecting) Connecting(const std::string& name); diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index d9dcd42c..5facd6aa 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -4,8 +4,9 @@ #include -#include #include +#include +#include #include #include @@ -19,7 +20,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory) namespace moveit { namespace task_constructor { -MOVEIT_CLASS_FORWARD(SubTrajectory) +class SolutionBase; MOVEIT_CLASS_FORWARD(InterfaceState) MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Stage) @@ -31,23 +32,24 @@ MOVEIT_CLASS_FORWARD(Stage) */ class InterfaceState { public: - typedef std::deque SubTrajectoryList; + // TODO turn this into priority queue + typedef std::deque Solutions; InterfaceState(const planning_scene::PlanningSceneConstPtr& ps); inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; } - inline const SubTrajectoryList& incomingTrajectories() const { return incoming_trajectories_; } - inline const SubTrajectoryList& outgoingTrajectories() const { return outgoing_trajectories_; } + inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; } + inline const Solutions& 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); } + inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } + inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); } private: planning_scene::PlanningSceneConstPtr scene_; // TODO: add PropertyMap: std::map to allow passing of parameters or attributes // TODO: add visualization_msgs::MarkerArray to allow for any complex visualization of the state - SubTrajectoryList incoming_trajectories_; - SubTrajectoryList outgoing_trajectories_; + Solutions incoming_trajectories_; + Solutions outgoing_trajectories_; }; @@ -64,7 +66,7 @@ public: // add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state // and finally run the notify callback - container_type::iterator add(InterfaceState &&state, SubTrajectory* incoming, SubTrajectory* outgoing); + container_type::iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); using container_type::value_type; using container_type::reference; @@ -95,31 +97,57 @@ private: }; -struct SubTrajectory { - SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) - : trajectory(traj), - begin(NULL), - end(NULL), - cost(0) - {} +class StagePrivate; +class SolutionBase { +public: + inline const StagePrivate* creator() const { return creator_; } + inline double cost() const { return cost_; } + + inline const InterfaceState* start() const { return start_; } + inline const InterfaceState* end() const { return end_; } inline void setStartState(const InterfaceState& state){ - assert(begin == NULL); - begin= &state; + assert(start_ == NULL); + start_ = &state; const_cast(state).addOutgoing(this); } inline void setEndState(const InterfaceState& state){ - assert(end == NULL); - end= &state; + assert(end_ == NULL); + end_ = &state; const_cast(state).addIncoming(this); } + void setCost(double cost); - // TODO: trajectories could have a name, e.g. a generator could name its solutions - const robot_trajectory::RobotTrajectoryPtr trajectory; - const InterfaceState* begin; - const InterfaceState* end; - double cost; +protected: + SolutionBase(StagePrivate* creator, double 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: + // back-pointer to creating stage, allows to access sub-solutions + StagePrivate *creator_; + // associated cost + double cost_; }; + +enum TraverseDirection { FORWARD, BACKWARD }; +template +const InterfaceState::Solutions& trajectories(const SolutionBase &start); + +template <> inline +const InterfaceState::Solutions& trajectories(const SolutionBase &solution) { + return solution.end()->outgoingTrajectories(); +} +template <> inline +const InterfaceState::Solutions& trajectories(const SolutionBase &solution) { + return solution.start()->incomingTrajectories(); +} + } } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 4b7268f3..c895f9ba 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -36,9 +36,10 @@ public: const moveit::core::RobotState &getCurrentRobotState() const; - typedef std::function&)> SolutionCallback; +#if 0 bool processSolutions(const SolutionCallback &processor); bool processSolutions(const SolutionCallback &processor) const; +#endif }; } } diff --git a/src/container.cpp b/src/container.cpp index 81f12295..00e0644a 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -1,49 +1,54 @@ #include "container_p.h" +#include + #include #include +#include +#include namespace moveit { namespace task_constructor { -ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before) const { +ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const { const_iterator position = children_.begin(); - if (before > 0) { - for (auto end = children_.end(); before > 0 && position != end; --before) + if (index > 0) { + for (auto end = children_.end(); index > 0 && position != end; --index) ++position; - } else if (++before <= 0) { + } else if (++index <= 0) { container_type::const_reverse_iterator from_end = children_.rbegin(); - for (auto end = children_.rend(); before < 0 && from_end != end; ++before) + for (auto end = children_.rend(); index < 0 && from_end != end; ++index) ++from_end; position = from_end.base(); } return position; } -inline bool ContainerBasePrivate::canInsert(const Stage &stage) const { - const StagePrivate* impl = stage.pimpl(); - return impl->parent() == nullptr // re-parenting is not supported - && impl->trajectories().empty(); // existing trajectories would become invalid -} +bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, + unsigned int cur_depth, unsigned int max_depth) const { + if (cur_depth >= max_depth) + return true; -bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, int depth) const { for (auto &stage : children_) { - if (!processor(*stage, depth)) + if (!processor(*stage, cur_depth)) continue; ContainerBasePrivate *container = dynamic_cast(stage->pimpl()); if (container) - container->traverseStages(processor, depth+1); + container->traverseStages(processor, cur_depth+1, max_depth); } return true; } -ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&stage, - ContainerBasePrivate::const_iterator pos) { - StagePrivate *impl = stage->pimpl(); - ContainerBasePrivate::iterator it = children_.insert(pos, std::move(stage)); - impl->setHierarchy(this, it); - return it; +bool ContainerBasePrivate::canCompute() const +{ + // call the method of the public interface + return static_cast(me_)->canCompute(); } +bool ContainerBasePrivate::compute() +{ + // call the method of the public interface + return static_cast(me_)->compute(); +} ContainerBase::ContainerBase(ContainerBasePrivate *impl) : Stage(impl) @@ -51,32 +56,41 @@ ContainerBase::ContainerBase(ContainerBasePrivate *impl) } PIMPL_FUNCTIONS(ContainerBase) +size_t ContainerBase::numChildren() const +{ + return pimpl()->children().size(); +} + +bool ContainerBase::traverseChildren(const ContainerBase::StageCallback &processor) const +{ + return pimpl()->traverseStages(processor, 0, 1); +} +bool ContainerBase::traverseRecursively(const ContainerBase::StageCallback &processor) const +{ + if (!processor(*this, 0)) + return false; + return pimpl()->traverseStages(processor, 1, UINT_MAX); +} + +bool ContainerBase::insert(Stage::pointer &&stage, int before) +{ + StagePrivate *impl = stage->pimpl(); + if (impl->parent() != nullptr || numSolutions() != 0) { + ROS_ERROR("cannot re-parent stage"); + return false; + } + + ContainerBasePrivate::const_iterator where = pimpl()->position(before); + ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage)); + impl->setHierarchy(pimpl(), it); + return true; +} + void ContainerBase::clear() { pimpl()->children_.clear(); } -bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) -{ - auto impl = pimpl(); - for (auto& stage : impl->children_) - stage->init(scene); -} - -bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const -{ - pimpl()->traverseStages(processor, 0); -} - -bool ContainerBase::canCompute() const -{ - pimpl()->canCompute(); -} - -bool ContainerBase::compute() { - pimpl()->compute(); -} - SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) : ContainerBasePrivate(me, name) @@ -93,13 +107,8 @@ InterfaceFlags SerialContainerPrivate::announcedFlags() const { return f; } -inline bool SerialContainerPrivate::canInsert(const Stage &stage, ContainerBasePrivate::const_iterator before) const { - if (!ContainerBasePrivate::canInsert(stage)) - return false; - return true; -} - -ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage, const_iterator before) +#if 0 +bool SerialContainerPrivate::init() { assert(canInsert(*stage, before)); bool at_begin = (before == children().begin()); @@ -128,9 +137,8 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage cur->setNextStarts(next->starts()); next->setPrevEnds(cur->ends()); } - - iterator it = ContainerBasePrivate::insert(std::move(stage), before); } +#endif inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const { @@ -144,6 +152,82 @@ inline ContainerBasePrivate::const_iterator SerialContainerPrivate::next(const_i return ++it; } + +struct SolutionCollector { + SolutionCollector(const Stage::pointer& stage) : stopping_stage(stage->pimpl()) {} + + bool operator()(const SolutionBase& current, const std::vector& trace, double cost) { + if (current.creator() != stopping_stage) + return true; // not yet traversed to end + + auto solution = trace; + if (!trace.empty()) { + // Only add current to non-empty trace (as last element). + // Empty trace indicates, that current connects to the start/end itself. + // In this case, we add current once in onNewSolution(), but not here! + solution.push_back(¤t); + cost += current.cost(); + } + + solutions.emplace_back(std::make_pair(std::move(solution), cost)); + return false; // we are done + } + + std::list, double>> solutions; + const StagePrivate* const stopping_stage; +}; + +void SerialContainerPrivate::onNewSolution(SolutionBase ¤t) +{ + const StagePrivate *creator = current.creator(); + std::cerr << "new solution:" << ¤t << " from " << creator << " " << creator->name() << std::endl; + + // s.creator() should be one of our children + assert(std::find_if(children().begin(), children().end(), + [creator](const Stage::pointer& stage) { return stage->pimpl() == creator; } ) + != children().end()); + + SerialContainer *me = static_cast(me_); + + // TODO: can we get rid of this and use a temporary when calling traverse()? + std::vector trace; trace.reserve(children().size()); + + // find all incoming trajectories connected to s + SolutionCollector incoming(children().front()); + me->traverse(current, std::ref(incoming), trace); + if (incoming.solutions.empty()) + return; // no connection to front() + +assert(trace.empty()); + // find all outgoing trajectories connected to s + SolutionCollector outgoing(children().back()); + me->traverse(current, std::ref(outgoing), trace); + if (outgoing.solutions.empty()) + return; // no connection to back() + + // add solutions for all combinations of incoming + s + outgoing + std::vector solution; + solution.reserve(children().size()); + for (auto& in : incoming.solutions) { + for (auto& out : outgoing.solutions) { + assert(solution.empty()); + // insert incoming solutions in reverse order + std::copy(in.first.rbegin(), in.first.rend(), solution.end()); + // insert current solution + solution.push_back(¤t); + // insert outgoing solutions in normal order + std::copy(out.first.begin(), out.first.end(), solution.end()); + + storeNewSolution(SerialSolution(this, std::move(solution), in.second + current.cost() + out.second)); + } + } +} + +void SerialContainerPrivate::storeNewSolution(SerialSolution&& s) +{ +} + + SerialContainer::SerialContainer(SerialContainerPrivate *impl) : ContainerBase(impl) {} @@ -152,36 +236,30 @@ SerialContainer::SerialContainer(const std::string &name) {} PIMPL_FUNCTIONS(SerialContainer) -bool SerialContainer::canInsert(const value_type& stage, int before) const +bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) { auto impl = pimpl(); - return impl->canInsert(*stage, impl->position(before)); + // recursively init all children + for (auto& stage : impl->children()) { + // derived classes should call Stage::init internally , but we cannot be sure... + if (!stage->init(scene) ||!stage->Stage::init(scene)) + return false; + } + return !impl->children().empty(); } -bool SerialContainer::insert(value_type&& stage, int before) +bool SerialContainer::canCompute() const { - auto impl = pimpl(); - - ContainerBasePrivate::const_iterator where = impl->position(before); - if (!impl->canInsert(*stage, where)) - return false; - - impl->insert(std::move(stage), where); - return true; + return !pimpl()->children().empty(); } -bool SerialContainerPrivate::canCompute() const -{ - return children().size() > 0; -} - -bool SerialContainerPrivate::compute() +bool SerialContainer::compute() { bool computed = false; - for(const auto& stage : children()) { + for(const auto& stage : pimpl()->children()) { if(!stage->pimpl()->canCompute()) continue; - std::cout << "Computing stage '" << stage->getName() << "':" << std::endl; + std::cout << "Computing stage '" << stage->name() << "':" << std::endl; bool success = stage->pimpl()->compute(); computed = true; std::cout << (success ? "succeeded" : "failed") << std::endl; @@ -189,4 +267,31 @@ bool SerialContainerPrivate::compute() return computed; } +size_t SerialContainer::numSolutions() const +{ + return pimpl()->solutions_.size(); +} + +template +bool SerialContainer::traverse(const SolutionBase &start, const SolutionCallback &cb, + std::vector &trace, double trace_cost) +{ + if (!cb(start, trace, trace_cost)) + return false; // stopping criterium met? + + bool result = false; // if no trajectory traversed, return false + for (SolutionBase* successor : trajectories(start)) { + trace.push_back(successor); + trace_cost += successor->cost(); + + result = traverse(*successor, cb, trace, trace_cost); + + trace_cost -= successor->cost(); + trace.pop_back(); + + if (!result) break; + } + return result; +} + } } diff --git a/src/container_p.h b/src/container_p.h index 0dae5720..a5243bc2 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -3,6 +3,9 @@ #include #include "stage_p.h" +#include +#include + namespace moveit { namespace task_constructor { class ContainerBasePrivate : public StagePrivate @@ -10,18 +13,27 @@ class ContainerBasePrivate : public StagePrivate friend class ContainerBase; public: - typedef ContainerBase::value_type value_type; typedef StagePrivate::container_type container_type; typedef container_type::iterator iterator; typedef container_type::const_iterator const_iterator; inline const container_type& children() const { return children_; } - const_iterator position(int before = -1) const; - bool canInsert(const Stage& stage) const; - virtual iterator insert(value_type &&stage, const_iterator pos); + // Retrieve iterator into children_ pointing to indexed element. + // Negative index counts from end(). + // Contrary to std::advance(), iterator limits are considered. + const_iterator position(int index) const; - bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const; + // traversing all stages upto max_depth + bool traverseStages(const ContainerBase::StageCallback &processor, + unsigned int cur_depth, unsigned int max_depth) const; + + // forward these methods to the public interface for containers + bool canCompute() const override; + bool compute() override; + + // callback when a new trajectory or combined solution becomes available + virtual void onNewSolution(SolutionBase& t) = 0; protected: ContainerBasePrivate(ContainerBase *me, const std::string &name) @@ -33,21 +45,51 @@ private: }; +class SerialSolution : public SolutionBase { +public: + explicit SerialSolution(StagePrivate* creator, std::vector&& subsolutions, double cost) + : SolutionBase(creator, cost), subsolutions_(subsolutions) + {} + +private: + // series of sub solutions + std::vector subsolutions_; +}; + + class SerialContainerPrivate : public ContainerBasePrivate { + friend class SerialContainer; + public: SerialContainerPrivate(SerialContainer* me, const std::string &name); InterfaceFlags announcedFlags() const override; - inline bool canInsert(const Stage& stage, const_iterator before) const; - virtual iterator insert(value_type &&stage, const_iterator before) override; - - bool canCompute() const override; - bool compute() override; + void onNewSolution(SolutionBase &t) override; + void storeNewSolution(SerialSolution &&s); private: inline const_iterator prev(const_iterator it) const; inline const_iterator next(const_iterator it) const; + + /* A container needs to decouple its interface from those of its children: + * A solution of a container needs to connect start to end via a full path. + * Start/end states of a single stage may only need to have a single outgoing/incoming trajectory. + * Note, that there might be many solutions connecting the same start-end state pair. */ + + // map first child's start states to the corresponding states in this' starts_ + std::map internal_to_my_starts_; + // map last child's end states to the corresponding states in this' ends_ + std::map internal_to_my_ends_; + + /* First/last childrens sendBackward()/sendForward() states are not directly propagated + * to previous/next stage of this container, because we cannot provide a solution yet. + * Only if we have full solution from start to end available, we can propagate the states */ + // interface to receive first child's sendBackward() states + InterfacePtr pending_backward_; + // interface to receive last child's sendForward() states + InterfacePtr pending_forward_; + + std::list solutions_; }; - } } diff --git a/src/debug.cpp b/src/debug.cpp index 7c40706b..cf23c9e4 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -9,12 +9,12 @@ namespace moveit { namespace task_constructor { bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, - const std::vector& solution, bool wait){ + const std::vector& solution, bool wait){ dt.trajectory.clear(); for(const SubTrajectory* t : solution){ - if(t->trajectory){ + if(t->trajectory()){ dt.trajectory.emplace_back(); - t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back()); + t->trajectory()->getRobotTrajectoryMsg(dt.trajectory.back()); } } @@ -34,10 +34,12 @@ void publishAllPlans(const Task &task, const std::string &topic, bool wait) { robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start); dt.model_id= task.getCurrentRobotState().getRobotModel()->getName(); +#if 0 Task::SolutionCallback processor = std::bind( &publishSolution, pub, dt, std::placeholders::_1, wait); task.processSolutions(processor); +#endif } @@ -54,6 +56,7 @@ void NewSolutionPublisher::publish() robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start); dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName(); +#if 0 Task::SolutionCallback processor = [this,&dt](const std::vector& solution) { bool all_published = true; for(const SubTrajectory* t : solution){ @@ -68,6 +71,7 @@ void NewSolutionPublisher::publish() return publishSolution(pub_, dt, solution, false); }; task_.processSolutions(processor); +#endif } } } diff --git a/src/stage.cpp b/src/stage.cpp index b658e4d6..4bd810ff 100644 --- a/src/stage.cpp +++ b/src/stage.cpp @@ -6,74 +6,15 @@ namespace moveit { namespace task_constructor { -Stage::Stage(StagePrivate *impl) - : pimpl_(impl) -{ -} - -Stage::~Stage() -{ - delete pimpl_; -} - -bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene) -{ -} - -const std::string& Stage::getName() const { - return pimpl_->name_; -} - -std::ostream& operator<<(std::ostream &os, const Stage& stage) { - os << *stage.pimpl(); - return os; -} - -template -const char* direction(const StagePrivate& stage) { - InterfaceFlags f = stage.deducedFlags(); - bool own_if = f & own; - bool other_if = f & other; - bool reverse = own & INPUT_IF_MASK; - if (own_if && other_if) return "<>"; - if (!own_if && !other_if) return "--"; - if (other_if ^ reverse) return "->"; - return "<-"; -}; - -std::ostream& operator<<(std::ostream &os, const StagePrivate& stage) { - // starts - for (const Interface* i : {stage.prev_ends_, stage.starts_.get()}) { - os << std::setw(3); - if (i) os << i->size(); - else os << "-"; - } - // trajectories - os << std::setw(5) << direction(stage) - << std::setw(3) << stage.trajectories_.size() - << std::setw(5) << direction(stage); - // ends - for (const Interface* i : {stage.ends_.get(), stage.next_starts_}) { - os << std::setw(3); - if (i) os << i->size(); - else os << "-"; - } - // name - os << " / " << stage.name_; - return os; -} +SubTrajectory::SubTrajectory(StagePrivate *creator, const robot_trajectory::RobotTrajectoryPtr &traj, double cost) + : SolutionBase(creator, cost), trajectory_(traj) +{} StagePrivate::StagePrivate(Stage *me, const std::string &name) : me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr) {} -SubTrajectory& StagePrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ - trajectories_.emplace_back(trajectory); - SubTrajectory& back = trajectories_.back(); - return back; -} - InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags result = announcedFlags(); @@ -94,8 +35,97 @@ inline InterfaceFlags StagePrivate::deducedFlags() const } +Stage::Stage(StagePrivate *impl) + : pimpl_(impl) +{ + assert(impl); +} + +Stage::~Stage() +{ + delete pimpl_; +} + + +inline bool implies(bool p, bool q) { return !p || q; } +bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene) +{ +#if 0 + auto impl = pimpl(); + InterfaceFlags f = impl->announcedFlags(); + if (!implies(f & WRITES_NEXT_START, impl->nextStarts())) + throw std::runtime_error("missing next unit for sendForward()"); + + if (!implies(f & WRITES_PREV_END, impl->prevEnds())) + throw std::runtime_error("missing previous unit for sendBackward()"); +#endif + return true; +} + +const std::string& Stage::name() const { + return pimpl_->name_; +} + +template +const char* direction(const StagePrivate& stage) { + InterfaceFlags f = stage.deducedFlags(); + bool own_if = f & own; + bool other_if = f & other; + bool reverse = own & INPUT_IF_MASK; + if (own_if && other_if) return "<>"; + if (!own_if && !other_if) return "--"; + if (other_if ^ reverse) return "->"; + return "<-"; +} + +std::ostream& operator<<(std::ostream &os, const Stage& stage) { + auto impl = stage.pimpl(); + // starts + for (const Interface* i : {impl->prevEnds(), impl->starts()}) { + os << std::setw(3); + if (i) os << i->size(); + else os << "-"; + } + // trajectories + os << std::setw(5) << direction(*impl) + << std::setw(3) << stage.numSolutions() + << std::setw(5) << direction(*impl); + // ends + for (const Interface* i : {impl->ends(), impl->nextStarts()}) { + os << std::setw(3); + if (i) os << i->size(); + else os << "-"; + } + // name + os << " / " << stage.name(); + return os; +} + + +ComputeBase::ComputeBase(ComputeBasePrivate *impl) + : Stage(impl) +{ +} +PIMPL_FUNCTIONS(ComputeBase) + +SubTrajectory& ComputeBase::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ + auto &trajs = pimpl()->trajectories_; + trajs.emplace_back(SubTrajectory(pimpl(), trajectory, cost)); + return trajs.back(); +} + +size_t ComputeBase::numSolutions() const { + return pimpl()->trajectories_.size(); +} + +const std::list &ComputeBase::trajectories() const +{ + return pimpl()->trajectories_; +} + + PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name) - : StagePrivate(me, name), dir(dir) + : ComputeBasePrivate(me, name), dir(dir) { } @@ -187,7 +217,7 @@ PropagatingEitherWay::PropagatingEitherWay(const std::string &name) } PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl) - : Stage(impl) + : ComputeBase(impl) { initInterface(); } @@ -228,15 +258,30 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir initInterface(); } +bool PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + auto impl = pimpl(); + if (!impl->nextStarts()) { + impl->starts_.reset(); + impl->next_start_state_ = Interface::iterator(); + } + if (!impl->prevEnds()) { + impl->ends_.reset(); + impl->next_end_state_ = Interface::iterator(); + } + return Stage::init(scene); +} + void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, const robot_trajectory::RobotTrajectoryPtr& t, double cost){ auto impl = pimpl(); std::cout << "sending state forward" << std::endl; - SubTrajectory &trajectory = impl->addTrajectory(t, cost); + SubTrajectory &trajectory = addTrajectory(t, cost); trajectory.setStartState(from); impl->nextStarts()->add(std::move(to), &trajectory, NULL); + impl->parent()->onNewSolution(trajectory); } void PropagatingEitherWay::sendBackward(InterfaceState&& from, @@ -245,9 +290,10 @@ void PropagatingEitherWay::sendBackward(InterfaceState&& from, double cost){ auto impl = pimpl(); std::cout << "sending state backward" << std::endl; - SubTrajectory& trajectory = impl->addTrajectory(t, cost); + SubTrajectory& trajectory = addTrajectory(t, cost); trajectory.setEndState(to); impl->prevEnds()->add(std::move(from), NULL, &trajectory); + impl->parent()->onNewSolution(trajectory); } @@ -292,7 +338,7 @@ bool PropagatingBackward::computeForward(const InterfaceState &from) GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name) - : StagePrivate(me, name) + : ComputeBasePrivate(me, name) {} InterfaceFlags GeneratorPrivate::announcedFlags() const { @@ -309,7 +355,7 @@ bool GeneratorPrivate::compute() { Generator::Generator(const std::string &name) - : Stage(new GeneratorPrivate(this, name)) + : ComputeBase(new GeneratorPrivate(this, name)) {} PIMPL_FUNCTIONS(Generator) @@ -319,14 +365,15 @@ void Generator::spawn(InterfaceState&& state, double cost) auto impl = pimpl(); // empty trajectory ref -> this node only produces states robot_trajectory::RobotTrajectoryPtr dummy; - SubTrajectory& trajectory = impl->addTrajectory(dummy, cost); + SubTrajectory& trajectory = addTrajectory(dummy, cost); impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory); impl->nextStarts()->add(std::move(state), &trajectory, NULL); + impl->parent()->onNewSolution(trajectory); } ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) - : StagePrivate(me, name) + : ComputeBasePrivate(me, name) { starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); })); ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); })); @@ -366,7 +413,7 @@ bool ConnectingPrivate::compute() { Connecting::Connecting(const std::string &name) - : Stage(new ConnectingPrivate(this, name)) + : ComputeBase(new ConnectingPrivate(this, name)) { } PIMPL_FUNCTIONS(Connecting) @@ -374,9 +421,10 @@ PIMPL_FUNCTIONS(Connecting) void Connecting::connect(const InterfaceState& from, const InterfaceState& to, const robot_trajectory::RobotTrajectoryPtr& t, double cost) { auto impl = pimpl(); - SubTrajectory& trajectory = impl->addTrajectory(t, cost); + SubTrajectory& trajectory = addTrajectory(t, cost); trajectory.setStartState(from); trajectory.setEndState(to); + impl->parent()->onNewSolution(trajectory); } } } diff --git a/src/stage_p.h b/src/stage_p.h index 3baf2541..66ddc65b 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -25,6 +25,7 @@ enum InterfaceFlag { }; typedef Flags InterfaceFlags; + class ContainerBasePrivate; class StagePrivate { friend class Stage; @@ -37,21 +38,19 @@ public: InterfaceFlags interfaceFlags() const; InterfaceFlags deducedFlags() const; virtual InterfaceFlags announcedFlags() const = 0; - std::list& trajectories() { return trajectories_; } virtual bool canCompute() const = 0; virtual bool compute() = 0; + inline const std::string& name() const { return name_; } 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 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; @@ -66,7 +65,6 @@ protected: InterfacePtr starts_; InterfacePtr ends_; - std::list trajectories_; private: // !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !! @@ -79,7 +77,22 @@ private: std::ostream& operator<<(std::ostream &os, const StagePrivate& stage); -class PropagatingEitherWayPrivate : public StagePrivate { +// ComputeBasePrivate is the base class for all computing stages, i.e. non-containers. +// It adds the trajectories_ variable. +class ComputeBasePrivate : public StagePrivate { + friend class ComputeBase; + +public: + ComputeBasePrivate(Stage* me, const std::string& name) + : StagePrivate(me, name) + {} + +private: + std::list trajectories_; +}; + + +class PropagatingEitherWayPrivate : public ComputeBasePrivate { friend class PropagatingEitherWay; public: @@ -120,7 +133,7 @@ public: }; -class GeneratorPrivate : public StagePrivate { +class GeneratorPrivate : public ComputeBasePrivate { public: inline GeneratorPrivate(Generator *me, const std::string &name); InterfaceFlags announcedFlags() const override; @@ -130,7 +143,7 @@ public: }; -class ConnectingPrivate : public StagePrivate { +class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; public: diff --git a/src/stages/current_state.cpp b/src/stages/current_state.cpp index 9dcedb78..dd6bd971 100644 --- a/src/stages/current_state.cpp +++ b/src/stages/current_state.cpp @@ -13,6 +13,7 @@ bool CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene) { scene_ = scene; ran_= false; + return true; } bool CurrentState::canCompute() const{ diff --git a/src/stages/generate_grasp_pose.cpp b/src/stages/generate_grasp_pose.cpp index 151177c1..81691c53 100644 --- a/src/stages/generate_grasp_pose.cpp +++ b/src/stages/generate_grasp_pose.cpp @@ -37,6 +37,7 @@ GenerateGraspPose::GenerateGraspPose(std::string name) bool GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) { scene_ = scene; + return true; } void GenerateGraspPose::setGroup(std::string group){ diff --git a/src/stages/gripper.cpp b/src/stages/gripper.cpp index ad830335..24c4424a 100644 --- a/src/stages/gripper.cpp +++ b/src/stages/gripper.cpp @@ -20,6 +20,7 @@ Gripper::Gripper(std::string name) bool Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) { planner_ = Task::createPlanner(scene->getRobotModel()); + return true; } void Gripper::setEndEffector(std::string eef){ diff --git a/src/stages/move.cpp b/src/stages/move.cpp index 74ad051a..5a61b8f4 100644 --- a/src/stages/move.cpp +++ b/src/stages/move.cpp @@ -21,6 +21,7 @@ Move::Move(std::string name) bool Move::init(const planning_scene::PlanningSceneConstPtr &scene) { planner_ = Task::createPlanner(scene->getRobotModel()); + return true; } void Move::setGroup(std::string group){ diff --git a/src/storage.cpp b/src/storage.cpp index 0f303491..a013747a 100644 --- a/src/storage.cpp +++ b/src/storage.cpp @@ -11,7 +11,7 @@ Interface::Interface(const Interface::NotifyFunction ¬ify) : notify_(notify) {} -Interface::iterator Interface::add(InterfaceState &&state, SubTrajectory* incoming, SubTrajectory* outgoing) { +Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing) { if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty()) throw std::runtime_error("expecting empty incoming/outgoing trajectories"); if (!state.scene()) @@ -28,4 +28,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SubTrajectory* incomi return back; } + +void SolutionBase::setCost(double cost) { + cost_ = cost; +} + } } diff --git a/src/task.cpp b/src/task.cpp index 7b515d06..1c07f901 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -93,7 +93,7 @@ void Task::add(pointer &&stage) { if (!stage) throw std::runtime_error("stage insertion failed: invalid stage pointer"); if (!insert(std::move(stage))) - throw std::runtime_error(std::string("insertion failed for stage: ") + stage->getName()); + throw std::runtime_error(std::string("insertion failed for stage: ") + stage->name()); } bool Task::plan(){ @@ -101,7 +101,11 @@ bool Task::plan(){ NewSolutionPublisher debug(*this); impl->initScene(); - this->init(impl->scene_); + if (!this->init(impl->scene_)) { + ROS_ERROR("init failed"); + return false; + } + while(ros::ok() && canCompute()) { if (compute()) { debug.publish(); @@ -109,7 +113,7 @@ bool Task::plan(){ } else break; } - return false; + return numSolutions() > 0; } const robot_state::RobotState& Task::getCurrentRobotState() const { @@ -122,38 +126,10 @@ void Task::printState(){ std::cout << std::string(2*depth, ' ') << stage << std::endl; return true; }; - traverseStages(processor); -} - -namespace { -bool traverseFullTrajectories( - SubTrajectory& start, - int nr_of_trajectories, - const Task::SolutionCallback& cb, - std::vector& trace) -{ - bool ret= true; - - trace.push_back(&start); - - if(nr_of_trajectories == 1){ - ret= cb(trace); - } - else if( start.end ){ - for( SubTrajectory* successor : start.end->outgoingTrajectories() ){ - if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){ - ret= false; - break; - } - } - } - - trace.pop_back(); - - return ret; -} + traverseRecursively(processor); } +#if 0 bool Task::processSolutions(const Task::SolutionCallback& processor) { auto impl = pimpl(); const TaskPrivate::container_type& children = impl->children(); @@ -169,5 +145,6 @@ bool Task::processSolutions(const Task::SolutionCallback& processor) { bool Task::processSolutions(const Task::SolutionCallback& processor) const { return const_cast(this)->processSolutions(processor); } +#endif } }