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:
|
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);
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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();
|
||||||
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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(¤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)
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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
|
||||||
}
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
192
src/stage.cpp
192
src/stage.cpp
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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:
|
||||||
|
|||||||
@ -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{
|
||||||
|
|||||||
@ -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){
|
||||||
|
|||||||
@ -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){
|
||||||
|
|||||||
@ -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){
|
||||||
|
|||||||
@ -11,7 +11,7 @@ Interface::Interface(const Interface::NotifyFunction ¬ify)
|
|||||||
: 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;
|
||||||
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
43
src/task.cpp
43
src/task.cpp
@ -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
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user