This commit is contained in:
Robert Haschke 2017-10-16 16:21:53 +02:00
parent f6a40a4a02
commit 6167f728ed
16 changed files with 520 additions and 240 deletions

View File

@ -9,20 +9,22 @@ class ContainerBase : public Stage
{ {
public: public:
PRIVATE_CLASS(ContainerBase) PRIVATE_CLASS(ContainerBase)
typedef Stage::pointer value_type;
size_t numChildren() const;
typedef std::function<bool(const Stage&, int depth)> StageCallback; typedef std::function<bool(const Stage&, int depth)> StageCallback;
typedef std::function<bool(const std::vector<SubTrajectory*>&)> 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(Stage::pointer&& stage, int before = -1);
virtual bool insert(value_type&& stage, int before = -1) = 0;
virtual void clear(); virtual void clear();
bool init(const planning_scene::PlanningSceneConstPtr &scene) override; virtual bool canCompute() const = 0;
bool canCompute() const; virtual bool compute() = 0;
bool compute();
bool traverseStages(const StageCallback &processor) const; size_t numSolutions() const = 0;
protected: protected:
ContainerBase(ContainerBasePrivate* impl); ContainerBase(ContainerBasePrivate* impl);
@ -35,8 +37,26 @@ public:
PRIVATE_CLASS(SerialContainer) PRIVATE_CLASS(SerialContainer)
SerialContainer(const std::string& name); SerialContainer(const std::string& name);
bool canInsert(const value_type& stage, int before = -1) const override; bool init(const planning_scene::PlanningSceneConstPtr &scene) override;
bool insert(value_type&& stage, int before = -1) 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<bool(const SolutionBase& current,
const std::vector<const SolutionBase*>& 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<TraverseDirection dir>
bool traverse(const SolutionBase &start, const SolutionCallback &cb,
std::vector<const SolutionBase*> &trace, double trace_cost = 0);
protected: protected:
SerialContainer(SerialContainerPrivate* impl); SerialContainer(SerialContainerPrivate* impl);

View File

@ -12,7 +12,7 @@ MOVEIT_CLASS_FORWARD(Task)
MOVEIT_CLASS_FORWARD(SubTrajectory) MOVEIT_CLASS_FORWARD(SubTrajectory)
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
const std::vector<SubTrajectory*>& solution, bool wait); const std::vector<const moveit::task_constructor::SubTrajectory *> &solution, bool wait);
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true); void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);

View File

@ -5,6 +5,7 @@
#include "utils.h" #include "utils.h"
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_task_constructor/storage.h>
#include <vector> #include <vector>
#include <list> #include <list>
@ -29,11 +30,26 @@ namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(Stage) MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(SubTrajectory)
class InterfaceState; class InterfaceState;
typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair; typedef std::pair<const InterfaceState&, const InterfaceState&> 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 StagePrivate;
class Stage { class Stage {
friend std::ostream& operator<<(std::ostream &os, const Stage& stage); friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
@ -41,26 +57,42 @@ class Stage {
public: public:
PRIVATE_CLASS(Stage) PRIVATE_CLASS(Stage)
typedef std::unique_ptr<Stage> pointer; typedef std::unique_ptr<Stage> pointer;
~Stage(); ~Stage();
/// initialize stage once before planning /// initialize stage once before planning
virtual bool init(const planning_scene::PlanningSceneConstPtr& scene); 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: protected:
/// Stage can only be instantiated through derived classes /// Stage can only be instantiated through derived classes
Stage(StagePrivate *impl); Stage(StagePrivate *impl);
protected: protected:
// TODO: use unique_ptr<StagePrivate> and get rid of destructor
StagePrivate* const pimpl_; // constness guarantees one initial write StagePrivate* const pimpl_; // constness guarantees one initial write
}; };
std::ostream& operator<<(std::ostream &os, const Stage& stage); 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<SubTrajectory>& trajectories() const;
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
};
class PropagatingEitherWayPrivate; class PropagatingEitherWayPrivate;
class PropagatingEitherWay : public Stage { class PropagatingEitherWay : public ComputeBase {
public: public:
PRIVATE_CLASS(PropagatingEitherWay) PRIVATE_CLASS(PropagatingEitherWay)
PropagatingEitherWay(const std::string& name); PropagatingEitherWay(const std::string& name);
@ -68,6 +100,7 @@ public:
enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD}; enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
void restrictDirection(Direction dir); void restrictDirection(Direction dir);
virtual bool init(const planning_scene::PlanningSceneConstPtr &scene) override;
virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeForward(const InterfaceState& from) = 0;
virtual bool computeBackward(const InterfaceState& to) = 0; virtual bool computeBackward(const InterfaceState& to) = 0;
@ -114,7 +147,7 @@ private:
class GeneratorPrivate; class GeneratorPrivate;
class Generator : public Stage { class Generator : public ComputeBase {
public: public:
PRIVATE_CLASS(Generator) PRIVATE_CLASS(Generator)
Generator(const std::string& name); Generator(const std::string& name);
@ -126,7 +159,7 @@ public:
class ConnectingPrivate; class ConnectingPrivate;
class Connecting : public Stage { class Connecting : public ComputeBase {
public: public:
PRIVATE_CLASS(Connecting) PRIVATE_CLASS(Connecting)
Connecting(const std::string& name); Connecting(const std::string& name);

View File

@ -4,8 +4,9 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <deque>
#include <list> #include <list>
#include <vector>
#include <deque>
#include <cassert> #include <cassert>
#include <functional> #include <functional>
@ -19,7 +20,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory)
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(SubTrajectory) class SolutionBase;
MOVEIT_CLASS_FORWARD(InterfaceState) MOVEIT_CLASS_FORWARD(InterfaceState)
MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(Stage) MOVEIT_CLASS_FORWARD(Stage)
@ -31,23 +32,24 @@ MOVEIT_CLASS_FORWARD(Stage)
*/ */
class InterfaceState { class InterfaceState {
public: public:
typedef std::deque<SubTrajectory*> SubTrajectoryList; // TODO turn this into priority queue
typedef std::deque<SolutionBase*> Solutions;
InterfaceState(const planning_scene::PlanningSceneConstPtr& ps); InterfaceState(const planning_scene::PlanningSceneConstPtr& ps);
inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; } inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
inline const SubTrajectoryList& incomingTrajectories() const { return incoming_trajectories_; } inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
inline const SubTrajectoryList& outgoingTrajectories() const { return outgoing_trajectories_; } inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
inline void addIncoming(SubTrajectory* t) { incoming_trajectories_.push_back(t); } inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
inline void addOutgoing(SubTrajectory* t) { outgoing_trajectories_.push_back(t); } inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
private: private:
planning_scene::PlanningSceneConstPtr scene_; planning_scene::PlanningSceneConstPtr scene_;
// TODO: add PropertyMap: std::map<std::string, std::any> to allow passing of parameters or attributes // TODO: add PropertyMap: std::map<std::string, std::any> to allow passing of parameters or attributes
// TODO: add visualization_msgs::MarkerArray to allow for any complex visualization of the state // TODO: add visualization_msgs::MarkerArray to allow for any complex visualization of the state
SubTrajectoryList incoming_trajectories_; Solutions incoming_trajectories_;
SubTrajectoryList outgoing_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 // add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
// and finally run the notify callback // 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::value_type;
using container_type::reference; using container_type::reference;
@ -95,31 +97,57 @@ private:
}; };
struct SubTrajectory { class StagePrivate;
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) class SolutionBase {
: trajectory(traj), public:
begin(NULL), inline const StagePrivate* creator() const { return creator_; }
end(NULL), inline double cost() const { return cost_; }
cost(0)
{} inline const InterfaceState* start() const { return start_; }
inline const InterfaceState* end() const { return end_; }
inline void setStartState(const InterfaceState& state){ inline void setStartState(const InterfaceState& state){
assert(begin == NULL); assert(start_ == NULL);
begin= &state; start_ = &state;
const_cast<InterfaceState&>(state).addOutgoing(this); const_cast<InterfaceState&>(state).addOutgoing(this);
} }
inline void setEndState(const InterfaceState& state){ inline void setEndState(const InterfaceState& state){
assert(end == NULL); assert(end_ == NULL);
end= &state; end_ = &state;
const_cast<InterfaceState&>(state).addIncoming(this); const_cast<InterfaceState&>(state).addIncoming(this);
} }
void setCost(double cost);
// TODO: trajectories could have a name, e.g. a generator could name its solutions protected:
const robot_trajectory::RobotTrajectoryPtr trajectory; SolutionBase(StagePrivate* creator, double cost)
const InterfaceState* begin; : creator_(creator), cost_(cost)
const InterfaceState* end; {}
double 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 <TraverseDirection dir>
const InterfaceState::Solutions& trajectories(const SolutionBase &start);
template <> inline
const InterfaceState::Solutions& trajectories<FORWARD>(const SolutionBase &solution) {
return solution.end()->outgoingTrajectories();
}
template <> inline
const InterfaceState::Solutions& trajectories<BACKWARD>(const SolutionBase &solution) {
return solution.start()->incomingTrajectories();
}
} } } }

View File

@ -36,9 +36,10 @@ public:
const moveit::core::RobotState &getCurrentRobotState() const; const moveit::core::RobotState &getCurrentRobotState() const;
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback; #if 0
bool processSolutions(const SolutionCallback &processor); bool processSolutions(const SolutionCallback &processor);
bool processSolutions(const SolutionCallback &processor) const; bool processSolutions(const SolutionCallback &processor) const;
#endif
}; };
} } } }

View File

@ -1,49 +1,54 @@
#include "container_p.h" #include "container_p.h"
#include <ros/console.h>
#include <memory> #include <memory>
#include <iostream> #include <iostream>
#include <algorithm>
#include <boost/range/adaptor/reversed.hpp>
namespace moveit { namespace task_constructor { 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(); const_iterator position = children_.begin();
if (before > 0) { if (index > 0) {
for (auto end = children_.end(); before > 0 && position != end; --before) for (auto end = children_.end(); index > 0 && position != end; --index)
++position; ++position;
} else if (++before <= 0) { } else if (++index <= 0) {
container_type::const_reverse_iterator from_end = children_.rbegin(); 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; ++from_end;
position = from_end.base(); position = from_end.base();
} }
return position; return position;
} }
inline bool ContainerBasePrivate::canInsert(const Stage &stage) const { bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor,
const StagePrivate* impl = stage.pimpl(); unsigned int cur_depth, unsigned int max_depth) const {
return impl->parent() == nullptr // re-parenting is not supported if (cur_depth >= max_depth)
&& impl->trajectories().empty(); // existing trajectories would become invalid return true;
}
bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, int depth) const {
for (auto &stage : children_) { for (auto &stage : children_) {
if (!processor(*stage, depth)) if (!processor(*stage, cur_depth))
continue; continue;
ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl()); ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl());
if (container) if (container)
container->traverseStages(processor, depth+1); container->traverseStages(processor, cur_depth+1, max_depth);
} }
return true; return true;
} }
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&stage, bool ContainerBasePrivate::canCompute() const
ContainerBasePrivate::const_iterator pos) { {
StagePrivate *impl = stage->pimpl(); // call the method of the public interface
ContainerBasePrivate::iterator it = children_.insert(pos, std::move(stage)); return static_cast<ContainerBase*>(me_)->canCompute();
impl->setHierarchy(this, it);
return it;
} }
bool ContainerBasePrivate::compute()
{
// call the method of the public interface
return static_cast<ContainerBase*>(me_)->compute();
}
ContainerBase::ContainerBase(ContainerBasePrivate *impl) ContainerBase::ContainerBase(ContainerBasePrivate *impl)
: Stage(impl) : Stage(impl)
@ -51,32 +56,41 @@ ContainerBase::ContainerBase(ContainerBasePrivate *impl)
} }
PIMPL_FUNCTIONS(ContainerBase) 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() void ContainerBase::clear()
{ {
pimpl()->children_.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) SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
@ -93,13 +107,8 @@ InterfaceFlags SerialContainerPrivate::announcedFlags() const {
return f; return f;
} }
inline bool SerialContainerPrivate::canInsert(const Stage &stage, ContainerBasePrivate::const_iterator before) const { #if 0
if (!ContainerBasePrivate::canInsert(stage)) bool SerialContainerPrivate::init()
return false;
return true;
}
ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage, const_iterator before)
{ {
assert(canInsert(*stage, before)); assert(canInsert(*stage, before));
bool at_begin = (before == children().begin()); bool at_begin = (before == children().begin());
@ -128,9 +137,8 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
cur->setNextStarts(next->starts()); cur->setNextStarts(next->starts());
next->setPrevEnds(cur->ends()); next->setPrevEnds(cur->ends());
} }
iterator it = ContainerBasePrivate::insert(std::move(stage), before);
} }
#endif
inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const
{ {
@ -144,6 +152,82 @@ inline ContainerBasePrivate::const_iterator SerialContainerPrivate::next(const_i
return ++it; return ++it;
} }
struct SolutionCollector {
SolutionCollector(const Stage::pointer& stage) : stopping_stage(stage->pimpl()) {}
bool operator()(const SolutionBase& current, const std::vector<const SolutionBase*>& 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(&current);
cost += current.cost();
}
solutions.emplace_back(std::make_pair(std::move(solution), cost));
return false; // we are done
}
std::list<std::pair<std::vector<const SolutionBase*>, double>> solutions;
const StagePrivate* const stopping_stage;
};
void SerialContainerPrivate::onNewSolution(SolutionBase &current)
{
const StagePrivate *creator = current.creator();
std::cerr << "new solution:" << &current << " 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<SerialContainer*>(me_);
// TODO: can we get rid of this and use a temporary when calling traverse()?
std::vector<const SolutionBase*> trace; trace.reserve(children().size());
// find all incoming trajectories connected to s
SolutionCollector incoming(children().front());
me->traverse<BACKWARD>(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<FORWARD>(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<const SolutionBase*> 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(&current);
// 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) SerialContainer::SerialContainer(SerialContainerPrivate *impl)
: ContainerBase(impl) : ContainerBase(impl)
{} {}
@ -152,36 +236,30 @@ SerialContainer::SerialContainer(const std::string &name)
{} {}
PIMPL_FUNCTIONS(SerialContainer) PIMPL_FUNCTIONS(SerialContainer)
bool SerialContainer::canInsert(const value_type& stage, int before) const bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
auto impl = pimpl(); 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(); return !pimpl()->children().empty();
ContainerBasePrivate::const_iterator where = impl->position(before);
if (!impl->canInsert(*stage, where))
return false;
impl->insert(std::move(stage), where);
return true;
} }
bool SerialContainerPrivate::canCompute() const bool SerialContainer::compute()
{
return children().size() > 0;
}
bool SerialContainerPrivate::compute()
{ {
bool computed = false; bool computed = false;
for(const auto& stage : children()) { for(const auto& stage : pimpl()->children()) {
if(!stage->pimpl()->canCompute()) if(!stage->pimpl()->canCompute())
continue; continue;
std::cout << "Computing stage '" << stage->getName() << "':" << std::endl; std::cout << "Computing stage '" << stage->name() << "':" << std::endl;
bool success = stage->pimpl()->compute(); bool success = stage->pimpl()->compute();
computed = true; computed = true;
std::cout << (success ? "succeeded" : "failed") << std::endl; std::cout << (success ? "succeeded" : "failed") << std::endl;
@ -189,4 +267,31 @@ bool SerialContainerPrivate::compute()
return computed; return computed;
} }
size_t SerialContainer::numSolutions() const
{
return pimpl()->solutions_.size();
}
template <TraverseDirection dir>
bool SerialContainer::traverse(const SolutionBase &start, const SolutionCallback &cb,
std::vector<const SolutionBase *> &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<dir>(start)) {
trace.push_back(successor);
trace_cost += successor->cost();
result = traverse<dir>(*successor, cb, trace, trace_cost);
trace_cost -= successor->cost();
trace.pop_back();
if (!result) break;
}
return result;
}
} } } }

View File

@ -3,6 +3,9 @@
#include <moveit_task_constructor/container.h> #include <moveit_task_constructor/container.h>
#include "stage_p.h" #include "stage_p.h"
#include <map>
#include <climits>
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
class ContainerBasePrivate : public StagePrivate class ContainerBasePrivate : public StagePrivate
@ -10,18 +13,27 @@ class ContainerBasePrivate : public StagePrivate
friend class ContainerBase; friend class ContainerBase;
public: public:
typedef ContainerBase::value_type value_type;
typedef StagePrivate::container_type container_type; typedef StagePrivate::container_type container_type;
typedef container_type::iterator iterator; typedef container_type::iterator iterator;
typedef container_type::const_iterator const_iterator; typedef container_type::const_iterator const_iterator;
inline const container_type& children() const { return children_; } inline const container_type& children() const { return children_; }
const_iterator position(int before = -1) const;
bool canInsert(const Stage& stage) const; // Retrieve iterator into children_ pointing to indexed element.
virtual iterator insert(value_type &&stage, const_iterator pos); // 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: protected:
ContainerBasePrivate(ContainerBase *me, const std::string &name) ContainerBasePrivate(ContainerBase *me, const std::string &name)
@ -33,21 +45,51 @@ private:
}; };
class SerialSolution : public SolutionBase {
public:
explicit SerialSolution(StagePrivate* creator, std::vector<const SolutionBase*>&& subsolutions, double cost)
: SolutionBase(creator, cost), subsolutions_(subsolutions)
{}
private:
// series of sub solutions
std::vector<const SolutionBase*> subsolutions_;
};
class SerialContainerPrivate : public ContainerBasePrivate { class SerialContainerPrivate : public ContainerBasePrivate {
friend class SerialContainer;
public: public:
SerialContainerPrivate(SerialContainer* me, const std::string &name); SerialContainerPrivate(SerialContainer* me, const std::string &name);
InterfaceFlags announcedFlags() const override; InterfaceFlags announcedFlags() const override;
inline bool canInsert(const Stage& stage, const_iterator before) const; void onNewSolution(SolutionBase &t) override;
virtual iterator insert(value_type &&stage, const_iterator before) override; void storeNewSolution(SerialSolution &&s);
bool canCompute() const override;
bool compute() override;
private: private:
inline const_iterator prev(const_iterator it) const; inline const_iterator prev(const_iterator it) const;
inline const_iterator next(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<Interface::iterator, Interface::iterator> internal_to_my_starts_;
// map last child's end states to the corresponding states in this' ends_
std::map<Interface::iterator, Interface::iterator> 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<SerialSolution> solutions_;
}; };
} } } }

View File

@ -9,12 +9,12 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
const std::vector<SubTrajectory*>& solution, bool wait){ const std::vector<const SubTrajectory*>& solution, bool wait){
dt.trajectory.clear(); dt.trajectory.clear();
for(const SubTrajectory* t : solution){ for(const SubTrajectory* t : solution){
if(t->trajectory){ if(t->trajectory()){
dt.trajectory.emplace_back(); 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); robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start);
dt.model_id= task.getCurrentRobotState().getRobotModel()->getName(); dt.model_id= task.getCurrentRobotState().getRobotModel()->getName();
#if 0
Task::SolutionCallback processor = std::bind( Task::SolutionCallback processor = std::bind(
&publishSolution, pub, dt, std::placeholders::_1, wait); &publishSolution, pub, dt, std::placeholders::_1, wait);
task.processSolutions(processor); task.processSolutions(processor);
#endif
} }
@ -54,6 +56,7 @@ void NewSolutionPublisher::publish()
robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start); robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start);
dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName(); dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName();
#if 0
Task::SolutionCallback processor = [this,&dt](const std::vector<SubTrajectory*>& solution) { Task::SolutionCallback processor = [this,&dt](const std::vector<SubTrajectory*>& solution) {
bool all_published = true; bool all_published = true;
for(const SubTrajectory* t : solution){ for(const SubTrajectory* t : solution){
@ -68,6 +71,7 @@ void NewSolutionPublisher::publish()
return publishSolution(pub_, dt, solution, false); return publishSolution(pub_, dt, solution, false);
}; };
task_.processSolutions(processor); task_.processSolutions(processor);
#endif
} }
} } } }

View File

@ -6,74 +6,15 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
Stage::Stage(StagePrivate *impl) SubTrajectory::SubTrajectory(StagePrivate *creator, const robot_trajectory::RobotTrajectoryPtr &traj, double cost)
: pimpl_(impl) : SolutionBase(creator, cost), trajectory_(traj)
{ {}
}
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<InterfaceFlag own, InterfaceFlag other>
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<READS_START, WRITES_PREV_END>(stage)
<< std::setw(3) << stage.trajectories_.size()
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(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;
}
StagePrivate::StagePrivate(Stage *me, const std::string &name) StagePrivate::StagePrivate(Stage *me, const std::string &name)
: me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr) : 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 StagePrivate::interfaceFlags() const
{ {
InterfaceFlags result = announcedFlags(); 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<InterfaceFlag own, InterfaceFlag other>
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<READS_START, WRITES_PREV_END>(*impl)
<< std::setw(3) << stage.numSolutions()
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(*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<SubTrajectory> &ComputeBase::trajectories() const
{
return pimpl()->trajectories_;
}
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name) 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) PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
: Stage(impl) : ComputeBase(impl)
{ {
initInterface(); initInterface();
} }
@ -228,15 +258,30 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
initInterface(); 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, void PropagatingEitherWay::sendForward(const InterfaceState& from,
InterfaceState&& to, InterfaceState&& to,
const robot_trajectory::RobotTrajectoryPtr& t, const robot_trajectory::RobotTrajectoryPtr& t,
double cost){ double cost){
auto impl = pimpl(); auto impl = pimpl();
std::cout << "sending state forward" << std::endl; std::cout << "sending state forward" << std::endl;
SubTrajectory &trajectory = impl->addTrajectory(t, cost); SubTrajectory &trajectory = addTrajectory(t, cost);
trajectory.setStartState(from); trajectory.setStartState(from);
impl->nextStarts()->add(std::move(to), &trajectory, NULL); impl->nextStarts()->add(std::move(to), &trajectory, NULL);
impl->parent()->onNewSolution(trajectory);
} }
void PropagatingEitherWay::sendBackward(InterfaceState&& from, void PropagatingEitherWay::sendBackward(InterfaceState&& from,
@ -245,9 +290,10 @@ void PropagatingEitherWay::sendBackward(InterfaceState&& from,
double cost){ double cost){
auto impl = pimpl(); auto impl = pimpl();
std::cout << "sending state backward" << std::endl; std::cout << "sending state backward" << std::endl;
SubTrajectory& trajectory = impl->addTrajectory(t, cost); SubTrajectory& trajectory = addTrajectory(t, cost);
trajectory.setEndState(to); trajectory.setEndState(to);
impl->prevEnds()->add(std::move(from), NULL, &trajectory); 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) GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
: StagePrivate(me, name) : ComputeBasePrivate(me, name)
{} {}
InterfaceFlags GeneratorPrivate::announcedFlags() const { InterfaceFlags GeneratorPrivate::announcedFlags() const {
@ -309,7 +355,7 @@ bool GeneratorPrivate::compute() {
Generator::Generator(const std::string &name) Generator::Generator(const std::string &name)
: Stage(new GeneratorPrivate(this, name)) : ComputeBase(new GeneratorPrivate(this, name))
{} {}
PIMPL_FUNCTIONS(Generator) PIMPL_FUNCTIONS(Generator)
@ -319,14 +365,15 @@ void Generator::spawn(InterfaceState&& state, double cost)
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;
SubTrajectory& trajectory = impl->addTrajectory(dummy, cost); SubTrajectory& trajectory = addTrajectory(dummy, cost);
impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory); impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory);
impl->nextStarts()->add(std::move(state), &trajectory, NULL); impl->nextStarts()->add(std::move(state), &trajectory, NULL);
impl->parent()->onNewSolution(trajectory);
} }
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) 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); })); starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(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) Connecting::Connecting(const std::string &name)
: Stage(new ConnectingPrivate(this, name)) : ComputeBase(new ConnectingPrivate(this, name))
{ {
} }
PIMPL_FUNCTIONS(Connecting) PIMPL_FUNCTIONS(Connecting)
@ -374,9 +421,10 @@ PIMPL_FUNCTIONS(Connecting)
void Connecting::connect(const InterfaceState& from, const InterfaceState& to, void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
const robot_trajectory::RobotTrajectoryPtr& t, double cost) { const robot_trajectory::RobotTrajectoryPtr& t, double cost) {
auto impl = pimpl(); auto impl = pimpl();
SubTrajectory& trajectory = impl->addTrajectory(t, cost); SubTrajectory& trajectory = addTrajectory(t, cost);
trajectory.setStartState(from); trajectory.setStartState(from);
trajectory.setEndState(to); trajectory.setEndState(to);
impl->parent()->onNewSolution(trajectory);
} }
} } } }

View File

@ -25,6 +25,7 @@ enum InterfaceFlag {
}; };
typedef Flags<InterfaceFlag> InterfaceFlags; typedef Flags<InterfaceFlag> InterfaceFlags;
class ContainerBasePrivate; class ContainerBasePrivate;
class StagePrivate { class StagePrivate {
friend class Stage; friend class Stage;
@ -37,21 +38,19 @@ public:
InterfaceFlags interfaceFlags() const; InterfaceFlags interfaceFlags() const;
InterfaceFlags deducedFlags() const; InterfaceFlags deducedFlags() const;
virtual InterfaceFlags announcedFlags() const = 0; virtual InterfaceFlags announcedFlags() const = 0;
std::list<SubTrajectory>& trajectories() { return trajectories_; }
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual bool compute() = 0; virtual bool compute() = 0;
inline const std::string& name() const { return name_; }
inline ContainerBasePrivate* parent() const { return parent_; } inline ContainerBasePrivate* parent() const { return parent_; }
inline container_type::iterator it() const { return it_; } inline container_type::iterator it() const { return it_; }
inline Interface* starts() const { return starts_.get(); } inline Interface* starts() const { return starts_.get(); }
inline Interface* ends() const { return ends_.get(); } inline Interface* ends() const { return ends_.get(); }
inline Interface* prevEnds() const { return prev_ends_; } inline Interface* prevEnds() const { return prev_ends_; }
inline Interface* nextStarts() const { return next_starts_; } inline Interface* nextStarts() const { return next_starts_; }
inline const std::list<SubTrajectory> trajectories() const { return trajectories_; }
inline bool isConnected() const { return prev_ends_ || next_starts_; } 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) { inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) {
parent_ = parent; parent_ = parent;
@ -66,7 +65,6 @@ protected:
InterfacePtr starts_; InterfacePtr starts_;
InterfacePtr ends_; InterfacePtr ends_;
std::list<SubTrajectory> trajectories_;
private: private:
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !! // !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
@ -79,7 +77,22 @@ private:
std::ostream& operator<<(std::ostream &os, const StagePrivate& stage); 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<SubTrajectory> trajectories_;
};
class PropagatingEitherWayPrivate : public ComputeBasePrivate {
friend class PropagatingEitherWay; friend class PropagatingEitherWay;
public: public:
@ -120,7 +133,7 @@ public:
}; };
class GeneratorPrivate : public StagePrivate { class GeneratorPrivate : public ComputeBasePrivate {
public: public:
inline GeneratorPrivate(Generator *me, const std::string &name); inline GeneratorPrivate(Generator *me, const std::string &name);
InterfaceFlags announcedFlags() const override; InterfaceFlags announcedFlags() const override;
@ -130,7 +143,7 @@ public:
}; };
class ConnectingPrivate : public StagePrivate { class ConnectingPrivate : public ComputeBasePrivate {
friend class Connecting; friend class Connecting;
public: public:

View File

@ -13,6 +13,7 @@ bool CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
scene_ = scene; scene_ = scene;
ran_= false; ran_= false;
return true;
} }
bool CurrentState::canCompute() const{ bool CurrentState::canCompute() const{

View File

@ -37,6 +37,7 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
bool GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) bool GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
scene_ = scene; scene_ = scene;
return true;
} }
void GenerateGraspPose::setGroup(std::string group){ void GenerateGraspPose::setGroup(std::string group){

View File

@ -20,6 +20,7 @@ Gripper::Gripper(std::string name)
bool Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) bool Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
planner_ = Task::createPlanner(scene->getRobotModel()); planner_ = Task::createPlanner(scene->getRobotModel());
return true;
} }
void Gripper::setEndEffector(std::string eef){ void Gripper::setEndEffector(std::string eef){

View File

@ -21,6 +21,7 @@ Move::Move(std::string name)
bool Move::init(const planning_scene::PlanningSceneConstPtr &scene) bool Move::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
planner_ = Task::createPlanner(scene->getRobotModel()); planner_ = Task::createPlanner(scene->getRobotModel());
return true;
} }
void Move::setGroup(std::string group){ void Move::setGroup(std::string group){

View File

@ -11,7 +11,7 @@ Interface::Interface(const Interface::NotifyFunction &notify)
: notify_(notify) : 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()) if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty())
throw std::runtime_error("expecting empty incoming/outgoing trajectories"); throw std::runtime_error("expecting empty incoming/outgoing trajectories");
if (!state.scene()) if (!state.scene())
@ -28,4 +28,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SubTrajectory* incomi
return back; return back;
} }
void SolutionBase::setCost(double cost) {
cost_ = cost;
}
} } } }

View File

@ -93,7 +93,7 @@ void Task::add(pointer &&stage) {
if (!stage) if (!stage)
throw std::runtime_error("stage insertion failed: invalid stage pointer"); throw std::runtime_error("stage insertion failed: invalid stage pointer");
if (!insert(std::move(stage))) 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(){ bool Task::plan(){
@ -101,7 +101,11 @@ bool Task::plan(){
NewSolutionPublisher debug(*this); NewSolutionPublisher debug(*this);
impl->initScene(); impl->initScene();
this->init(impl->scene_); if (!this->init(impl->scene_)) {
ROS_ERROR("init failed");
return false;
}
while(ros::ok() && canCompute()) { while(ros::ok() && canCompute()) {
if (compute()) { if (compute()) {
debug.publish(); debug.publish();
@ -109,7 +113,7 @@ bool Task::plan(){
} else } else
break; break;
} }
return false; return numSolutions() > 0;
} }
const robot_state::RobotState& Task::getCurrentRobotState() const { const robot_state::RobotState& Task::getCurrentRobotState() const {
@ -122,38 +126,10 @@ void Task::printState(){
std::cout << std::string(2*depth, ' ') << stage << std::endl; std::cout << std::string(2*depth, ' ') << stage << std::endl;
return true; return true;
}; };
traverseStages(processor); traverseRecursively(processor);
}
namespace {
bool traverseFullTrajectories(
SubTrajectory& start,
int nr_of_trajectories,
const Task::SolutionCallback& cb,
std::vector<SubTrajectory*>& 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;
}
} }
#if 0
bool Task::processSolutions(const Task::SolutionCallback& processor) { bool Task::processSolutions(const Task::SolutionCallback& processor) {
auto impl = pimpl(); auto impl = pimpl();
const TaskPrivate::container_type& children = impl->children(); 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 { bool Task::processSolutions(const Task::SolutionCallback& processor) const {
return const_cast<Task*>(this)->processSolutions(processor); return const_cast<Task*>(this)->processSolutions(processor);
} }
#endif
} } } }