mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
wip
This commit is contained in:
parent
f6a40a4a02
commit
6167f728ed
@ -9,20 +9,22 @@ class ContainerBase : public Stage
|
||||
{
|
||||
public:
|
||||
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 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(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<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:
|
||||
SerialContainer(SerialContainerPrivate* impl);
|
||||
|
||||
@ -12,7 +12,7 @@ MOVEIT_CLASS_FORWARD(Task)
|
||||
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||
|
||||
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);
|
||||
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
|
||||
#include "utils.h"
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
#include <vector>
|
||||
#include <list>
|
||||
|
||||
@ -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<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 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<Stage> 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<StagePrivate> 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<SubTrajectory>& 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);
|
||||
|
||||
@ -4,8 +4,9 @@
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
||||
#include <deque>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <cassert>
|
||||
#include <functional>
|
||||
|
||||
@ -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<SubTrajectory*> SubTrajectoryList;
|
||||
// TODO turn this into priority queue
|
||||
typedef std::deque<SolutionBase*> 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<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
|
||||
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<InterfaceState&>(state).addOutgoing(this);
|
||||
}
|
||||
|
||||
inline void setEndState(const InterfaceState& state){
|
||||
assert(end == NULL);
|
||||
end= &state;
|
||||
assert(end_ == NULL);
|
||||
end_ = &state;
|
||||
const_cast<InterfaceState&>(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 <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();
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -36,9 +36,10 @@ public:
|
||||
|
||||
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) const;
|
||||
#endif
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -1,49 +1,54 @@
|
||||
#include "container_p.h"
|
||||
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <boost/range/adaptor/reversed.hpp>
|
||||
|
||||
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<ContainerBasePrivate*>(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<ContainerBase*>(me_)->canCompute();
|
||||
}
|
||||
|
||||
bool ContainerBasePrivate::compute()
|
||||
{
|
||||
// call the method of the public interface
|
||||
return static_cast<ContainerBase*>(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<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(¤t);
|
||||
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 ¤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<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(¤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));
|
||||
}
|
||||
|
||||
bool SerialContainer::insert(value_type&& stage, int before)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
|
||||
ContainerBasePrivate::const_iterator where = impl->position(before);
|
||||
if (!impl->canInsert(*stage, where))
|
||||
// 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;
|
||||
|
||||
impl->insert(std::move(stage), where);
|
||||
return true;
|
||||
}
|
||||
return !impl->children().empty();
|
||||
}
|
||||
|
||||
bool SerialContainerPrivate::canCompute() const
|
||||
bool SerialContainer::canCompute() const
|
||||
{
|
||||
return children().size() > 0;
|
||||
return !pimpl()->children().empty();
|
||||
}
|
||||
|
||||
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 <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;
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -3,6 +3,9 @@
|
||||
#include <moveit_task_constructor/container.h>
|
||||
#include "stage_p.h"
|
||||
|
||||
#include <map>
|
||||
#include <climits>
|
||||
|
||||
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<const SolutionBase*>&& subsolutions, double cost)
|
||||
: SolutionBase(creator, cost), subsolutions_(subsolutions)
|
||||
{}
|
||||
|
||||
private:
|
||||
// series of sub solutions
|
||||
std::vector<const SolutionBase*> 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<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_;
|
||||
};
|
||||
|
||||
|
||||
} }
|
||||
|
||||
@ -9,12 +9,12 @@
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
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();
|
||||
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<SubTrajectory*>& 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
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
192
src/stage.cpp
192
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<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;
|
||||
}
|
||||
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<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)
|
||||
: 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);
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -25,6 +25,7 @@ enum InterfaceFlag {
|
||||
};
|
||||
typedef Flags<InterfaceFlag> 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<SubTrajectory>& 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<SubTrajectory> 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<SubTrajectory> 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<SubTrajectory> 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:
|
||||
|
||||
@ -13,6 +13,7 @@ bool CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
scene_ = scene;
|
||||
ran_= false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CurrentState::canCompute() const{
|
||||
|
||||
@ -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){
|
||||
|
||||
@ -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){
|
||||
|
||||
@ -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){
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
43
src/task.cpp
43
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<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;
|
||||
}
|
||||
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<Task*>(this)->processSolutions(processor);
|
||||
}
|
||||
#endif
|
||||
|
||||
} }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user