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:
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);

View File

@ -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);

View File

@ -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);

View File

@ -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();
}
} }

View File

@ -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
};
} }

View File

@ -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(&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)
: 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;
}
} }

View File

@ -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_;
};
} }

View File

@ -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
}
} }

View File

@ -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);
}
} }

View File

@ -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:

View File

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

View File

@ -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){

View File

@ -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){

View File

@ -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){

View File

@ -11,7 +11,7 @@ Interface::Interface(const Interface::NotifyFunction &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())
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;
}
} }

View File

@ -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
} }