simplified SubTask API

- moved interface flags to private implementation
- moved newInputState() / newOutputState to private implementation
- directly implement sendBackward()/sendForward()
- removed scene_, planner_ from SubTaskPrivate
  ... replaced by init(scene) call
- renamings
  - array_type -> container_type
  - ...AnyWay -> ...EitherWay
  - input -> start
  - output -> end
This commit is contained in:
Robert Haschke 2017-10-09 09:41:12 +02:00
parent c085534ea9
commit 1b1a82f7c8
21 changed files with 503 additions and 508 deletions

View File

@ -30,6 +30,7 @@ add_library(${PROJECT_NAME}
src/debug.cpp
src/subtask_p.h
src/container_p.h
include/${PROJECT_NAME}/utils.h
include/${PROJECT_NAME}/storage.h
include/${PROJECT_NAME}/subtask.h

View File

@ -18,6 +18,10 @@ public:
virtual bool insert(value_type&& stage, int before = -1) = 0;
virtual void clear();
bool init(const planning_scene::PlanningSceneConstPtr &scene) override;
bool canCompute() const;
bool compute();
bool traverseStages(const StageCallback &processor) const;
protected:
@ -34,9 +38,6 @@ public:
bool canInsert(const value_type& stage, int before = -1) const override;
bool insert(value_type&& stage, int before = -1) override;
bool canCompute() const override;
bool compute() override;
protected:
SerialContainer(SerialContainerPrivate* impl);
};

View File

@ -42,41 +42,17 @@ public:
inline SubTaskPrivate* pimpl_func() { return pimpl_; }
typedef std::unique_ptr<SubTask> pointer;
enum InterfaceFlag {
READS_INPUT = 0x01,
READS_OUTPUT = 0x02,
WRITES_NEXT_INPUT = 0x04,
WRITES_PREV_OUTPUT = 0x08,
OWN_IF_MASK = READS_INPUT | READS_OUTPUT,
EXT_IF_MASK = WRITES_NEXT_INPUT | WRITES_PREV_OUTPUT,
INPUT_IF_MASK = READS_INPUT | WRITES_PREV_OUTPUT,
OUTPUT_IF_MASK = READS_OUTPUT | WRITES_NEXT_INPUT,
};
typedef Flags<InterfaceFlag> InterfaceFlags;
InterfaceFlags interfaceFlags() const;
~SubTask();
/// initialize stage once before planning
virtual bool init(const planning_scene::PlanningSceneConstPtr& scene);
const std::string& getName() const;
// TODO: results from { TIMEOUT, EXHAUSTED, FINISHED, WAITING }
virtual bool canCompute() const = 0;
virtual bool compute() = 0;
planning_scene::PlanningSceneConstPtr scene() const;
planning_pipeline::PlanningPipelinePtr planner() const;
void setPlanningScene(const planning_scene::PlanningSceneConstPtr&);
void setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr&);
protected:
/// can only instantiated by derived classes
/// SubTask can only be instantiated through derived classes
SubTask(SubTaskPrivate *impl);
/// methods called when a new InterfaceState was spawned
virtual void newInputState(const std::list<InterfaceState>::iterator&) {}
virtual void newOutputState(const std::list<InterfaceState>::iterator&) {}
protected:
// TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor
SubTaskPrivate* const pimpl_; // constness guarantees one initial write
@ -84,55 +60,57 @@ protected:
std::ostream& operator<<(std::ostream &os, const SubTask& stage);
class PropagatingAnyWayPrivate;
class PropagatingAnyWay : public SubTask {
class PropagatingEitherWayPrivate;
class PropagatingEitherWay : public SubTask {
public:
PRIVATE_CLASS(PropagatingAnyWay)
PropagatingAnyWay(const std::string& name);
PRIVATE_CLASS(PropagatingEitherWay)
PropagatingEitherWay(const std::string& name);
enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
void restrictDirection(Direction dir);
virtual bool computeForward(const InterfaceState& from, planning_scene::PlanningScenePtr& to,
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost);
virtual bool computeBackward(planning_scene::PlanningScenePtr& from, const InterfaceState& to,
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost);
virtual bool computeForward(const InterfaceState& from) = 0;
virtual bool computeBackward(const InterfaceState& to) = 0;
bool canCompute() const override;
bool compute() override;
void sendForward(const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to,
const robot_trajectory::RobotTrajectoryPtr& trajectory,
double cost = 0);
void sendBackward(const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to,
const robot_trajectory::RobotTrajectoryPtr& trajectory,
double cost = 0);
protected:
// constructor for use in derived classes
PropagatingAnyWay(PropagatingAnyWayPrivate* impl);
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
void initInterface();
// get informed when new beginnings and endings become available
void newInputState(const std::list<InterfaceState>::iterator& it);
void newOutputState(const std::list<InterfaceState>::iterator& it);
};
class PropagatingForwardPrivate;
class PropagatingForward : public PropagatingAnyWay {
class PropagatingForward : public PropagatingEitherWay {
public:
PRIVATE_CLASS(PropagatingForward)
PropagatingForward(const std::string& name);
private:
// restrict access to backward method to provide compile-time check
using PropagatingAnyWay::computeBackward;
bool computeBackward(const InterfaceState &to) override;
using PropagatingEitherWay::sendBackward;
};
class PropagatingBackwardPrivate;
class PropagatingBackward : public PropagatingAnyWay {
class PropagatingBackward : public PropagatingEitherWay {
public:
PRIVATE_CLASS(PropagatingBackward)
PropagatingBackward(const std::string& name);
private:
// restrict access to forward method to provide compile-time check
using PropagatingAnyWay::computeForward;
bool computeForward(const InterfaceState &from) override;
using PropagatingEitherWay::sendForward;
};
@ -142,7 +120,9 @@ public:
PRIVATE_CLASS(Generator)
Generator(const std::string& name);
bool spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
virtual bool canCompute() const = 0;
virtual bool compute() = 0;
void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
};
@ -152,15 +132,9 @@ public:
PRIVATE_CLASS(Connecting)
Connecting(const std::string& name);
// methods for manual use
bool hasStatePair() const;
InterfaceStatePair fetchStatePair();
void connect(const robot_trajectory::RobotTrajectoryPtr&,
const InterfaceStatePair&, double cost = 0);
protected:
virtual void newInputState(const std::list<InterfaceState>::iterator& it);
virtual void newOutputState(const std::list<InterfaceState>::iterator& it);
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
void connect(const InterfaceState& from, const InterfaceState& to,
const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost = 0);
};

View File

@ -16,14 +16,12 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
namespace moveit { namespace task_constructor { namespace subtasks {
class CartesianPositionMotion : public PropagatingAnyWay {
class CartesianPositionMotion : public PropagatingEitherWay {
public:
CartesianPositionMotion(std::string name);
virtual bool computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) override;
virtual bool computeBackward(planning_scene::PlanningScenePtr &from, const InterfaceState &to,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) override;
virtual bool computeForward(const InterfaceState &from) override;
virtual bool computeBackward(const InterfaceState &to) override;
void setGroup(std::string group);
void setLink(std::string link);
@ -57,7 +55,7 @@ protected:
ros::Publisher pub;
void _publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start);
void _publishTrajectory(const planning_scene::PlanningSceneConstPtr &scene, const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start);
};
} } }

View File

@ -10,10 +10,12 @@ class CurrentState : public Generator {
public:
CurrentState(std::string name);
virtual bool canCompute() const override;
virtual bool compute() override;
bool init(const planning_scene::PlanningSceneConstPtr& scene) override;
bool canCompute() const override;
bool compute() override;
protected:
planning_scene::PlanningSceneConstPtr scene_;
bool ran_;
};

View File

@ -18,8 +18,9 @@ class GenerateGraspPose : public Generator {
public:
GenerateGraspPose(std::string name);
virtual bool canCompute() const override;
virtual bool compute() override;
bool init(const planning_scene::PlanningSceneConstPtr& scene) override;
bool canCompute() const override;
bool compute() override;
void setEndEffector(std::string eef);
@ -42,6 +43,8 @@ public:
void ignoreCollisions(bool flag);
protected:
planning_scene::PlanningSceneConstPtr scene_;
std::string eef_;
std::string group_;

View File

@ -10,16 +10,13 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface)
namespace moveit { namespace task_constructor { namespace subtasks {
class Gripper : public PropagatingAnyWay {
class Gripper : public PropagatingEitherWay {
public:
Gripper(std::string name);
bool compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
bool computeForward(const InterfaceState& from, planning_scene::PlanningScenePtr &to,
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost) override;
bool computeBackward(planning_scene::PlanningScenePtr& from, const InterfaceState& to,
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost) override;
bool init(const planning_scene::PlanningSceneConstPtr &scene);
bool computeForward(const InterfaceState& from) override;
bool computeBackward(const InterfaceState& to) override;
void setEndEffector(std::string eef);
void setAttachLink(std::string link);
@ -29,12 +26,17 @@ public:
void graspObject(std::string grasp_object);
protected:
bool compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
protected:
std::string eef_;
std::string named_target_;
std::string grasp_object_;
std::string attach_link_;
planning_pipeline::PlanningPipelinePtr planner_;
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
};

View File

@ -14,29 +14,19 @@ class Move : public Connecting {
public:
Move(std::string name);
virtual bool canCompute() const override;
virtual bool compute() override;
bool init(const planning_scene::PlanningSceneConstPtr &scene);
bool compute(const InterfaceState &from, const InterfaceState &to);
void setGroup(std::string group);
void setLink(std::string link);
void setPlannerId(std::string planner);
void setTimeout(double timeout);
void setFrom(std::string named_target);
void setTo(std::string named_target);
protected:
std::string group_;
std::string link_;
std::string planner_id_;
double timeout_;
std::string planner_id_;
std::string from_named_target_;
std::string to_named_target_;
planning_pipeline::PlanningPipelinePtr planner_;
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
};

View File

@ -1,4 +1,5 @@
// copyright Michael 'v4hn' Goerner @ 2017
// copyright Robert Haschke @ 2017
#pragma once
@ -6,11 +7,8 @@
#include <moveit/macros/class_forward.h>
#include <ros/ros.h>
#include <vector>
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotState)
}}
@ -24,9 +22,11 @@ class TaskPrivate;
class Task : protected SerialContainer {
PRIVATE_CLASS(Task)
public:
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
Task(const std::string &name = std::string());
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
const std::string &ns = "move_group",
const std::string &planning_plugin_param_name = "planning_plugin",
const std::string &adapter_plugins_param_name = "request_adapters");
void add(SubTask::pointer &&stage);
using SerialContainer::clear;
@ -35,6 +35,8 @@ public:
void printState();
const moveit::core::RobotState &getCurrentRobotState() const;
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
bool processSolutions(const SolutionCallback &processor);
bool processSolutions(const SolutionCallback &processor) const;
};

View File

@ -11,7 +11,7 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
for (auto end = children_.end(); before > 0 && position != end; --before)
++position;
} else if (++before <= 0) {
array_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)
++from_end;
position = from_end.base();
@ -29,9 +29,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
for (auto &stage : children_) {
if (!processor(*stage, depth))
continue;
ContainerBase *container = dynamic_cast<ContainerBase*>(stage.get());
ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl_func());
if (container)
static_cast<ContainerBasePrivate*>(container->pimpl_func())->traverseStages(processor, depth+1);
container->traverseStages(processor, depth+1);
}
return true;
}
@ -40,8 +40,6 @@ ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate
ContainerBasePrivate::const_iterator pos) {
SubTaskPrivate *impl = subtask->pimpl_func();
impl->parent_ = this;
subtask->setPlanningScene(scene_);
subtask->setPlanningPipeline(planner_);
impl->it_ = children_.insert(pos, std::move(subtask));
return impl->it_;
}
@ -58,28 +56,46 @@ void ContainerBase::clear()
impl->clear();
}
bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
{
IMPL(ContainerBase);
for (auto& stage : impl->children_)
stage->init(scene);
}
bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
{
IMPL(const ContainerBase);
return impl->traverseStages(processor, 0);
}
bool ContainerBase::canCompute() const
{
IMPL(const ContainerBase);
return impl->canCompute();
}
SubTask::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
SubTask::InterfaceFlags f;
bool ContainerBase::compute() {
IMPL(ContainerBase);
return impl->compute();
}
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
InterfaceFlags f;
if (children().empty()) return f;
f |= children().front()->pimpl_func()->announcedFlags() & SubTask::INPUT_IF_MASK;
f |= children().back()->pimpl_func()->announcedFlags() & SubTask::OUTPUT_IF_MASK;
f |= children().front()->pimpl_func()->announcedFlags() & INPUT_IF_MASK;
f |= children().back()->pimpl_func()->announcedFlags() & OUTPUT_IF_MASK;
return f;
}
inline bool isConnectable(int prev_flags, int next_flags) {
return ((prev_flags & SubTask::WRITES_NEXT_INPUT) && (next_flags & SubTask::READS_INPUT)) ||
((prev_flags & SubTask::READS_OUTPUT) && (next_flags & SubTask::WRITES_PREV_OUTPUT));
return ((prev_flags & SubTaskPrivate::WRITES_NEXT_START) && (next_flags & SubTaskPrivate::READS_START)) ||
((prev_flags & SubTaskPrivate::READS_END) && (next_flags & SubTaskPrivate::WRITES_PREV_END));
}
inline bool bothWrite(SubTask::InterfaceFlags prev_flags, SubTask::InterfaceFlags next_flags) {
return (prev_flags.testFlag(SubTask::WRITES_NEXT_INPUT) && !next_flags.testFlag(SubTask::READS_INPUT)) &&
(next_flags.testFlag(SubTask::WRITES_PREV_OUTPUT) && !prev_flags.testFlag(SubTask::READS_OUTPUT));
inline bool bothWrite(SubTaskPrivate::InterfaceFlags prev_flags, SubTaskPrivate::InterfaceFlags next_flags) {
return (prev_flags.testFlag(SubTaskPrivate::WRITES_NEXT_START) && !next_flags.testFlag(SubTaskPrivate::READS_START)) &&
(next_flags.testFlag(SubTaskPrivate::WRITES_PREV_END) && !prev_flags.testFlag(SubTaskPrivate::READS_END));
}
inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBasePrivate::const_iterator before) const {
@ -89,9 +105,9 @@ inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBas
// check connectedness
bool at_end = (before == children().end());
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl_func();
SubTask::InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags();
SubTask::InterfaceFlags next_flags = next->deducedFlags();
SubTask::InterfaceFlags prev_flags = prev(before)->deducedFlags();
InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags();
InterfaceFlags next_flags = next->deducedFlags();
InterfaceFlags prev_flags = prev(before)->deducedFlags();
// Do a simple check here only. A full connectivity check requires the full pipeline to be setup
// Thus, here we reject when trying to connect to writers with each other
@ -108,27 +124,27 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
bool at_end = (before == children().end());
SubTaskPrivate *cur = stage->pimpl_func();
/* set pointer cache (prev_ouput_ and next_input_) of prev, current, and next stage */
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
if (children().empty()) { // first child inserted
setPrevOutput(cur, this->input_);
setNextInput(cur, this->output_);
setPrevEnds(cur, this->starts_);
setNextStarts(cur, this->ends_);
} else if (at_begin) {
SubTaskPrivate *next = (*before)->pimpl_func();
setPrevOutput(cur, this->input_);
setNextInput(cur, next->input_);
setPrevOutput(next, cur->output_);
setPrevEnds(cur, this->starts_);
setNextStarts(cur, next->starts_);
setPrevEnds(next, cur->ends_);
} else if (at_end) {
const SubTaskPrivate *prev = this->prev(before);
setNextInput(prev, cur->input_);
setPrevOutput(cur, prev->output_);
setNextInput(cur, this->output_);
setNextStarts(prev, cur->starts_);
setPrevEnds(cur, prev->ends_);
setNextStarts(cur, this->ends_);
} else {
const SubTaskPrivate *prev = this->prev(before);
SubTaskPrivate *next = (*before)->pimpl_func();
setNextInput(prev, cur->input_);
setPrevOutput(cur, prev->output_);
setNextInput(cur, next->input_);
setPrevOutput(next, cur->output_);
setNextStarts(prev, cur->starts_);
setPrevEnds(cur, prev->ends_);
setNextStarts(cur, next->starts_);
setPrevEnds(next, cur->ends_);
}
iterator it = ContainerBasePrivate::insert(std::move(stage), before);
@ -195,21 +211,19 @@ bool SerialContainer::insert(value_type&& subtask, int before)
return true;
}
bool SerialContainer::canCompute() const
bool SerialContainerPrivate::canCompute() const
{
IMPL(const SerialContainer);
return impl->children().size() > 0;
return children().size() > 0;
}
bool SerialContainer::compute()
bool SerialContainerPrivate::compute()
{
IMPL(SerialContainer);
bool computed = false;
for(const auto& stage : impl->children()){
if(!stage->canCompute())
for(const auto& stage : children()) {
if(!stage->pimpl_func()->canCompute())
continue;
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;
bool success = stage->compute();
bool success = stage->pimpl_func()->compute();
computed = true;
std::cout << (success ? "succeeded" : "failed") << std::endl;
}

View File

@ -7,14 +7,16 @@ namespace moveit { namespace task_constructor {
class ContainerBasePrivate : public SubTaskPrivate
{
friend class ContainerBase;
friend class BaseTest; // allow access for unit tests
public:
typedef ContainerBase::value_type value_type;
typedef SubTaskPrivate::array_type array_type;
typedef array_type::iterator iterator;
typedef array_type::const_iterator const_iterator;
typedef SubTaskPrivate::container_type container_type;
typedef container_type::iterator iterator;
typedef container_type::const_iterator const_iterator;
inline const array_type& children() const { return children_; }
inline const container_type& children() const { return children_; }
const_iterator position(int before = -1) const;
bool canInsert(const SubTask& stage) const;
@ -30,15 +32,15 @@ protected:
inline const ContainerBasePrivate* parent(const SubTaskPrivate *child) const { return child->parent_; }
inline iterator it(const SubTaskPrivate *child) const { return child->it_; }
inline void setPrevOutput(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->prev_output_ = interface.get();
inline void setPrevEnds(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->prev_ends_ = interface.get();
}
inline void setNextInput(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->next_input_ = interface.get();
inline void setNextStarts(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->next_starts_ = interface.get();
}
private:
array_type children_;
container_type children_;
};
@ -47,14 +49,17 @@ public:
SerialContainerPrivate(SerialContainer* me, const std::string &name)
: ContainerBasePrivate(me, name)
{
input_.reset(new Interface(Interface::NotifyFunction()));
output_.reset(new Interface(Interface::NotifyFunction()));
starts_.reset(new Interface(Interface::NotifyFunction()));
ends_.reset(new Interface(Interface::NotifyFunction()));
}
SubTask::InterfaceFlags announcedFlags() const override;
InterfaceFlags announcedFlags() const override;
bool canInsert(const SubTask& stage, const_iterator before) const;
virtual iterator insert(value_type &&stage, const_iterator before) override;
bool canCompute() const override;
bool compute() override;
inline const SubTaskPrivate *prev(const_iterator it) const;
inline const SubTaskPrivate *next(const_iterator it) const;

View File

@ -55,7 +55,6 @@ int main(int argc, char** argv){
{
auto move= std::make_unique<subtasks::Move>("move to pre-grasp");
move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame");
move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0);
t.add(std::move(move));

View File

@ -11,19 +11,15 @@ SubTask::SubTask(SubTaskPrivate *impl)
{
}
SubTask::InterfaceFlags SubTask::interfaceFlags() const
{
SubTask::InterfaceFlags result = pimpl_->announcedFlags();
result &= ~SubTask::InterfaceFlags(SubTask::OWN_IF_MASK);
result |= pimpl_->deducedFlags();
return result;
}
SubTask::~SubTask()
{
delete pimpl_;
}
bool SubTask::init(const planning_scene::PlanningSceneConstPtr &scene)
{
}
const std::string& SubTask::getName() const {
return pimpl_->name_;
}
@ -33,12 +29,12 @@ std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
return os;
}
template<SubTask::InterfaceFlag own, SubTask::InterfaceFlag other>
template<SubTaskPrivate::InterfaceFlag own, SubTaskPrivate::InterfaceFlag other>
const char* direction(const SubTaskPrivate& stage) {
SubTask::InterfaceFlags f = stage.deducedFlags();
SubTaskPrivate::InterfaceFlags f = stage.deducedFlags();
bool own_if = f & own;
bool other_if = f & other;
bool reverse = own & SubTask::INPUT_IF_MASK;
bool reverse = own & SubTaskPrivate::INPUT_IF_MASK;
if (own_if && other_if) return "<>";
if (!own_if && !other_if) return "--";
if (other_if ^ reverse) return "->";
@ -46,18 +42,18 @@ const char* direction(const SubTaskPrivate& stage) {
};
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
// inputs
for (const Interface* i : {stage.prev_output_, stage.input_.get()}) {
// 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<SubTask::READS_INPUT, SubTask::WRITES_PREV_OUTPUT>(stage)
os << std::setw(5) << direction<SubTaskPrivate::READS_START, SubTaskPrivate::WRITES_PREV_END>(stage)
<< std::setw(3) << stage.trajectories_.size()
<< std::setw(5) << direction<SubTask::READS_OUTPUT, SubTask::WRITES_NEXT_INPUT>(stage);
// outputs
for (const Interface* i : {stage.output_.get(), stage.next_input_}) {
<< std::setw(5) << direction<SubTaskPrivate::READS_END, SubTaskPrivate::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 << "-";
@ -67,164 +63,162 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
return os;
}
planning_scene::PlanningSceneConstPtr SubTask::scene() const
{
return pimpl_->scene_;
}
planning_pipeline::PlanningPipelinePtr SubTask::planner() const
{
return pimpl_->planner_;
}
void SubTask::setPlanningScene(const planning_scene::PlanningSceneConstPtr &scene){
pimpl_->scene_= scene;
}
void SubTask::setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr &planner){
pimpl_->planner_= planner;
}
SubTaskPrivate::SubTaskPrivate(SubTask *me, const std::__cxx11::string &name)
: me_(me), name_(name), parent_(nullptr), prev_output_(nullptr), next_input_(nullptr)
SubTaskPrivate::SubTaskPrivate(SubTask *me, const std::string &name)
: me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr)
{}
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
trajectories_.emplace_back(trajectory);
return trajectories_.back();
SubTrajectory& back = trajectories_.back();
return back;
}
SubTaskPrivate::InterfaceFlags SubTaskPrivate::interfaceFlags() const
{
InterfaceFlags result = announcedFlags();
result &= ~InterfaceFlags(OWN_IF_MASK);
result |= deducedFlags();
return result;
}
// return the interface flags that can be deduced from the interface
inline SubTask::InterfaceFlags SubTaskPrivate::deducedFlags() const
inline SubTaskPrivate::InterfaceFlags SubTaskPrivate::deducedFlags() const
{
SubTask::InterfaceFlags f;
if (input_) f |= SubTask::READS_INPUT;
if (output_) f |= SubTask::READS_OUTPUT;
if (prevOutput()) f |= SubTask::WRITES_PREV_OUTPUT;
if (nextInput()) f |= SubTask::WRITES_NEXT_INPUT;
InterfaceFlags f;
if (starts_) f |= READS_START;
if (ends_) f |= READS_END;
if (prevEnds()) f |= WRITES_PREV_END;
if (nextStarts()) f |= WRITES_NEXT_START;
return f;
}
inline bool SubTaskPrivate::sendForward(SubTrajectory &trajectory, const planning_scene::PlanningSceneConstPtr &ps){
std::cout << "sending state to start" << std::endl;
if (next_input_) {
next_input_->add(ps, &trajectory, NULL);
return true;
}
return false;
}
inline bool SubTaskPrivate::sendBackward(SubTrajectory &trajectory, const planning_scene::PlanningSceneConstPtr &ps){
std::cout << "sending state to end" << std::endl;
if (prev_output_) {
prev_output_->add(ps, NULL, &trajectory);
return true;
}
return false;
}
PropagatingAnyWayPrivate::PropagatingAnyWayPrivate(PropagatingAnyWay *me, PropagatingAnyWay::Direction dir, const std::__cxx11::string &name)
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
: SubTaskPrivate(me, name), dir(dir)
{
}
SubTask::InterfaceFlags PropagatingAnyWayPrivate::announcedFlags() const {
SubTask::InterfaceFlags f;
if (dir & PropagatingAnyWay::FORWARD)
f |= SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::WRITES_NEXT_INPUT});
if (dir & PropagatingAnyWay::BACKWARD)
f |= SubTask::InterfaceFlags({SubTask::READS_OUTPUT, SubTask::WRITES_PREV_OUTPUT});
SubTaskPrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
InterfaceFlags f;
if (dir & PropagatingEitherWay::FORWARD)
f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
if (dir & PropagatingEitherWay::BACKWARD)
f |= InterfaceFlags({READS_END, WRITES_PREV_END});
return f;
}
inline bool PropagatingAnyWayPrivate::hasStartState() const{
return next_input_state_ != input_->end();
inline bool PropagatingEitherWayPrivate::hasStartState() const{
return next_start_state_ != starts_->end();
}
const InterfaceState& PropagatingAnyWayPrivate::fetchStartState(){
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
if (!hasStartState())
throw std::runtime_error("no new state for beginning available");
const InterfaceState& state= *next_input_state_;
++next_input_state_;
const InterfaceState& state= *next_start_state_;
++next_start_state_;
return state;
}
bool PropagatingAnyWayPrivate::sendForward(const robot_trajectory::RobotTrajectoryPtr& t,
const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to,
double cost){
SubTrajectory &trajectory = addTrajectory(t, cost);
trajectory.setStartState(from);
return SubTaskPrivate::sendForward(trajectory, to);
inline bool PropagatingEitherWayPrivate::hasEndState() const{
return next_end_state_ != ends_->end();
}
inline bool PropagatingAnyWayPrivate::hasEndState() const{
return next_output_state_ != output_->end();
}
const InterfaceState& PropagatingAnyWayPrivate::fetchEndState(){
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
if(!hasEndState())
throw std::runtime_error("no new state for ending available");
const InterfaceState& state= *next_output_state_;
++next_output_state_;
const InterfaceState& state= *next_end_state_;
++next_end_state_;
return state;
}
bool PropagatingAnyWayPrivate::sendBackward(const robot_trajectory::RobotTrajectoryPtr& t,
const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to,
double cost){
SubTrajectory& trajectory = addTrajectory(t, cost);
trajectory.setEndState(to);
return SubTaskPrivate::sendBackward(trajectory, from);
bool PropagatingEitherWayPrivate::canCompute() const
{
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
return true;
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState())
return true;
return false;
}
bool PropagatingEitherWayPrivate::compute()
{
PropagatingEitherWay* me = static_cast<PropagatingEitherWay*>(me_);
bool result = false;
planning_scene::PlanningScenePtr ps;
robot_trajectory::RobotTrajectoryPtr trajectory;
double cost;
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
if (me->computeForward(fetchStartState()))
result |= true;
}
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
if (me->computeBackward(fetchEndState()))
result |= true;
}
return result;
}
void PropagatingEitherWayPrivate::newStartState(const Interface::iterator &it)
{
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if(next_start_state_ == starts_->end())
--next_start_state_;
}
void PropagatingEitherWayPrivate::newEndState(const Interface::iterator &it)
{
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if(next_end_state_ == ends_->end())
--next_end_state_;
}
PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
: PropagatingAnyWay(new PropagatingAnyWayPrivate(this, ANYWAY, name))
PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
{
}
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl)
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
: SubTask(impl)
{
initInterface();
}
void PropagatingAnyWay::initInterface()
void PropagatingEitherWay::initInterface()
{
IMPL(PropagatingAnyWay)
if (impl->dir & PropagatingAnyWay::FORWARD) {
if (!impl->input_) { // keep existing interface if possible
impl->input_.reset(new Interface([this](const Interface::iterator& it) { newInputState(it); }));
impl->next_input_state_ = impl->input_->begin();
IMPL(PropagatingEitherWay)
if (impl->dir & PropagatingEitherWay::FORWARD) {
if (!impl->starts_) { // keep existing interface if possible
impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); }));
impl->next_start_state_ = impl->starts_->begin();
}
} else {
impl->input_.reset();
impl->next_input_state_ = Interface::iterator();
impl->starts_.reset();
impl->next_start_state_ = Interface::iterator();
}
if (impl->dir & PropagatingAnyWay::BACKWARD) {
if (!impl->output_) { // keep existing interface if possible
impl->output_.reset(new Interface([this](const Interface::iterator& it) { newOutputState(it); }));
impl->next_output_state_ = impl->output_->end();
if (impl->dir & PropagatingEitherWay::BACKWARD) {
if (!impl->ends_) { // keep existing interface if possible
impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); }));
impl->next_end_state_ = impl->ends_->end();
}
} else {
impl->output_.reset();
impl->next_output_state_ = Interface::iterator();
impl->ends_.reset();
impl->next_end_state_ = Interface::iterator();
}
}
void PropagatingAnyWay::restrictDirection(PropagatingAnyWay::Direction dir)
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir)
{
IMPL(PropagatingAnyWay);
IMPL(PropagatingEitherWay);
if (impl->dir == dir) return;
if (impl->isConnected())
throw std::runtime_error("Cannot change direction after being connected");
@ -232,117 +226,81 @@ void PropagatingAnyWay::restrictDirection(PropagatingAnyWay::Direction dir)
initInterface();
}
bool PropagatingAnyWay::computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost)
{
// default implementation is void, needs override by user
return false;
void PropagatingEitherWay::sendForward(const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to,
const robot_trajectory::RobotTrajectoryPtr& t,
double cost){
IMPL(PropagatingEitherWay)
std::cout << "sending state forward" << std::endl;
SubTrajectory &trajectory = impl->addTrajectory(t, cost);
trajectory.setStartState(from);
impl->nextStarts()->add(to, &trajectory, NULL);
}
bool PropagatingAnyWay::computeBackward(planning_scene::PlanningScenePtr &from, const InterfaceState &to,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost)
{
// default implementation is void, needs override by user
return false;
}
bool PropagatingAnyWay::canCompute() const
{
IMPL(const PropagatingAnyWay);
if ((impl->dir & PropagatingAnyWay::FORWARD) && impl->hasStartState())
return true;
if ((impl->dir & PropagatingAnyWay::BACKWARD) && impl->hasEndState())
return true;
return false;
}
bool PropagatingAnyWay::compute()
{
IMPL(PropagatingAnyWay);
bool result = false;
planning_scene::PlanningScenePtr ps;
robot_trajectory::RobotTrajectoryPtr trajectory;
double cost;
if ((impl->dir & PropagatingAnyWay::FORWARD) && impl->hasStartState()) {
const InterfaceState& from = impl->fetchStartState();
if (computeForward(from, ps, trajectory, cost) && ps
&& impl->sendForward(trajectory, from, ps, cost))
result |= true;
}
if ((impl->dir & PropagatingAnyWay::BACKWARD) && impl->hasEndState()) {
const InterfaceState& to = impl->fetchEndState();
if (computeBackward(ps, to, trajectory, cost) && ps
&& impl->sendBackward(trajectory, ps, to, cost))
result |= true;
}
return result;
}
void PropagatingAnyWay::newInputState(const Interface::iterator &it)
{
IMPL(PropagatingAnyWay);
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if(impl->next_input_state_ == impl->input_->end())
--impl->next_input_state_;
}
void PropagatingAnyWay::newOutputState(const Interface::iterator &it)
{
IMPL(PropagatingAnyWay);
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if( impl->next_output_state_ == impl->output_->end() )
--impl->next_output_state_;
void PropagatingEitherWay::sendBackward(const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to,
const robot_trajectory::RobotTrajectoryPtr& t,
double cost){
IMPL(PropagatingEitherWay)
std::cout << "sending state backward" << std::endl;
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
trajectory.setEndState(to);
impl->prevEnds()->add(from, NULL, &trajectory);
}
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, const std::__cxx11::string &name)
: PropagatingAnyWayPrivate(me, PropagatingAnyWay::FORWARD, name)
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, const std::string &name)
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::FORWARD, name)
{
// indicate, that we don't accept new states from output interface
output_.reset();
next_output_state_ = Interface::iterator();
// indicate, that we don't accept new states from ends_ interface
ends_.reset();
next_end_state_ = Interface::iterator();
}
PropagatingForward::PropagatingForward(const std::string& name)
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name))
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name))
{}
PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, const std::__cxx11::string &name)
: PropagatingAnyWayPrivate(me, PropagatingAnyWay::BACKWARD, name)
bool PropagatingForward::computeBackward(const InterfaceState &to)
{
// indicate, that we don't accept new states from input interface
input_.reset();
next_input_state_ = Interface::iterator();
assert(false); // This should never be called
}
PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name)
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::BACKWARD, name)
{
// indicate, that we don't accept new states from starts_ interface
starts_.reset();
next_start_state_ = Interface::iterator();
}
PropagatingBackward::PropagatingBackward(const std::string &name)
: PropagatingAnyWay(new PropagatingBackwardPrivate(this, name))
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name))
{}
bool PropagatingBackward::computeForward(const InterfaceState &from)
{
assert(false); // This should never be called
}
GeneratorPrivate::GeneratorPrivate(Generator *me, const std::__cxx11::string &name)
GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
: SubTaskPrivate(me, name)
{}
SubTask::InterfaceFlags GeneratorPrivate::announcedFlags() const {
return SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT,SubTask::WRITES_PREV_OUTPUT});
SubTaskPrivate::InterfaceFlags GeneratorPrivate::announcedFlags() const {
return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END});
}
inline bool GeneratorPrivate::spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost)
{
// empty trajectory ref -> this node only produces states
robot_trajectory::RobotTrajectoryPtr dummy;
SubTrajectory& trajectory = addTrajectory(dummy, cost);
bool GeneratorPrivate::canCompute() const {
return static_cast<Generator*>(me_)->canCompute();
}
std::cout << "spawning state" << std::endl;
bool result = sendBackward(trajectory, ps);
result |= sendForward(trajectory, ps);
return result;
bool GeneratorPrivate::compute() {
return static_cast<Generator*>(me_)->compute();
}
@ -350,30 +308,55 @@ Generator::Generator(const std::string &name)
: SubTask(new GeneratorPrivate(this, name))
{}
bool Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
{
std::cout << "spawning state forwards and backwards" << std::endl;
IMPL(Generator)
return impl->spawn(ps, cost);
// empty trajectory ref -> this node only produces states
robot_trajectory::RobotTrajectoryPtr dummy;
SubTrajectory& trajectory = impl->addTrajectory(dummy, cost);
impl->prevEnds()->add(ps, NULL, &trajectory);
impl->nextStarts()->add(ps, &trajectory, NULL);
}
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::__cxx11::string &name)
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
: SubTaskPrivate(me, name)
{
input_.reset(new Interface(std::bind(&Connecting::newInputState, me, std::placeholders::_1)));
output_.reset(new Interface(std::bind(&Connecting::newOutputState, me, std::placeholders::_1)));
it_pairs_ = std::make_pair(input_->begin(), output_->begin());
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
}
SubTask::InterfaceFlags ConnectingPrivate::announcedFlags() const {
return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::READS_OUTPUT});
SubTaskPrivate::InterfaceFlags ConnectingPrivate::announcedFlags() const {
return InterfaceFlags({READS_START, READS_END});
}
inline void ConnectingPrivate::connect(const robot_trajectory::RobotTrajectoryPtr &t, const InterfaceStatePair &state_pair, double cost)
void ConnectingPrivate::newStartState(const Interface::iterator& it)
{
SubTrajectory& trajectory = addTrajectory(t, cost);
trajectory.setStartState(state_pair.first);
trajectory.setEndState(state_pair.second);
// TODO: need to handle the pairs iterator
if(it_pairs_.first == starts_->end())
--it_pairs_.first;
}
void ConnectingPrivate::newEndState(const Interface::iterator& it)
{
// TODO: need to handle the pairs iterator properly
if(it_pairs_.second == ends_->end())
--it_pairs_.second;
}
bool ConnectingPrivate::canCompute() const{
// TODO: implement this properly
return it_pairs_.first != starts_->end() &&
it_pairs_.second != ends_->end();
}
bool ConnectingPrivate::compute() {
// TODO: implement this properly
const InterfaceState& from = *it_pairs_.first;
const InterfaceState& to = *(it_pairs_.second++);
return static_cast<Connecting*>(me_)->compute(from, to);
}
@ -382,40 +365,12 @@ Connecting::Connecting(const std::string &name)
{
}
void Connecting::newInputState(const Interface::iterator& it)
{
IMPL(Connecting);
// TODO: need to handle the pairs iterator
if( impl->it_pairs_.first == impl->input_->end() )
--impl->it_pairs_.first;
}
void Connecting::newOutputState(const Interface::iterator& it)
{
IMPL(Connecting);
// TODO: need to handle the pairs iterator properly
if( impl->it_pairs_.second == impl->output_->end() )
--impl->it_pairs_.second;
}
bool Connecting::hasStatePair() const{
IMPL(const Connecting);
// TODO: implement this properly
return impl->it_pairs_.first != impl->input_->end() &&
impl->it_pairs_.second != impl->output_->end();
}
InterfaceStatePair Connecting::fetchStatePair(){
IMPL(Connecting);
// TODO: implement this properly
return std::pair<const InterfaceState&, const InterfaceState&>
(*impl->it_pairs_.first, *(impl->it_pairs_.second++));
}
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
{
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
const robot_trajectory::RobotTrajectoryPtr& t, double cost) {
IMPL(Connecting)
impl->connect(t, state_pair, cost);
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
trajectory.setStartState(from);
trajectory.setEndState(to);
}
} }

View File

@ -15,104 +15,106 @@ class SubTaskPrivate {
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
public:
typedef std::list<SubTask::pointer> array_type;
typedef std::list<SubTask::pointer> container_type;
SubTaskPrivate(SubTask* me, const std::string& name);
SubTask::InterfaceFlags deducedFlags() const;
virtual SubTask::InterfaceFlags announcedFlags() const = 0;
enum InterfaceFlag {
READS_START = 0x01,
READS_END = 0x02,
WRITES_NEXT_START = 0x04,
WRITES_PREV_END = 0x08,
OWN_IF_MASK = READS_START | READS_END,
EXT_IF_MASK = WRITES_NEXT_START | WRITES_PREV_END,
INPUT_IF_MASK = READS_START | WRITES_PREV_END,
OUTPUT_IF_MASK = READS_END | WRITES_NEXT_START,
};
typedef Flags<InterfaceFlag> InterfaceFlags;
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;
public:
SubTask* const me_; // associated/owning SubTask instance
const std::string name_;
planning_scene::PlanningSceneConstPtr scene_;
planning_pipeline::PlanningPipelinePtr planner_;
InterfacePtr starts_;
InterfacePtr ends_;
InterfacePtr input_;
InterfacePtr output_;
inline const Interface* prevOutput() const { return prev_output_; }
inline const Interface* nextInput() const { return next_input_; }
inline bool isConnected() const { return prev_output_ || next_input_; }
inline ContainerBasePrivate* parent() const { return parent_; }
inline bool isConnected() const { return prev_ends_ || next_starts_; }
inline Interface* prevEnds() const { return prev_ends_; }
inline Interface* nextStarts() const { return next_starts_; }
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
protected:
std::list<SubTrajectory> trajectories_;
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
bool sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
bool sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
private:
// !! items accessed only by ContainerBasePrivate to maintain hierarchy !!
ContainerBasePrivate* parent_; // owning parent
array_type::iterator it_; // iterator into parent's children_ list referring to this
// caching the pointers to the output_ / input_ interface of previous / next stage
mutable Interface *prev_output_; // interface to be used for sendBackward()
mutable Interface *next_input_; // interface to be use for sendForward()
container_type::iterator it_; // iterator into parent's children_ list referring to this
// caching the pointers to the ends_ / starts_ interface of previous / next stage
mutable Interface *prev_ends_; // interface to be used for sendBackward()
mutable Interface *next_starts_; // interface to be use for sendForward()
};
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
class PropagatingAnyWayPrivate : public SubTaskPrivate {
friend class PropagatingAnyWay;
class PropagatingEitherWayPrivate : public SubTaskPrivate {
friend class PropagatingEitherWay;
public:
PropagatingAnyWay::Direction dir;
PropagatingEitherWay::Direction dir;
inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, PropagatingAnyWay::Direction dir,
inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir,
const std::string &name);
SubTask::InterfaceFlags announcedFlags() const override;
InterfaceFlags announcedFlags() const override;
bool canCompute() const override;
bool compute() override;
bool hasStartState() const;
const InterfaceState &fetchStartState();
bool sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to,
double cost = 0);
bool hasEndState() const;
const InterfaceState &fetchEndState();
bool sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to,
double cost = 0);
protected:
Interface::const_iterator next_input_state_;
Interface::const_iterator next_output_state_;
// get informed when new start or end state becomes available
void newStartState(const std::list<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::iterator& it);
Interface::const_iterator next_start_state_;
Interface::const_iterator next_end_state_;
};
class PropagatingForwardPrivate : public PropagatingAnyWayPrivate {
class PropagatingForwardPrivate : public PropagatingEitherWayPrivate {
public:
inline PropagatingForwardPrivate(PropagatingForward *me, const std::string &name);
private:
// restrict access to backward methods to provide compile-time check
using PropagatingAnyWayPrivate::hasEndState;
using PropagatingAnyWayPrivate::fetchEndState;
using PropagatingAnyWayPrivate::sendBackward;
};
class PropagatingBackwardPrivate : public PropagatingAnyWayPrivate {
class PropagatingBackwardPrivate : public PropagatingEitherWayPrivate {
public:
inline PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name);
private:
// restrict access to forward method to provide compile-time check
using PropagatingAnyWayPrivate::hasStartState;
using PropagatingAnyWayPrivate::fetchStartState;
using PropagatingAnyWayPrivate::sendForward;
};
class GeneratorPrivate : public SubTaskPrivate {
public:
inline GeneratorPrivate(Generator *me, const std::string &name);
SubTask::InterfaceFlags announcedFlags() const override;
bool spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost);
InterfaceFlags announcedFlags() const override;
bool canCompute() const override;
bool compute() override;
};
@ -121,11 +123,19 @@ class ConnectingPrivate : public SubTaskPrivate {
public:
inline ConnectingPrivate(Connecting *me, const std::string &name);
SubTask::InterfaceFlags announcedFlags() const override;
InterfaceFlags announcedFlags() const override;
bool canCompute() const override;
bool compute() override;
void connect(const robot_trajectory::RobotTrajectoryPtr& t,
const InterfaceStatePair& state_pair, double cost);
private:
// get informed when new start or end state becomes available
void newStartState(const std::list<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::iterator& it);
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
};

View File

@ -16,7 +16,7 @@
namespace moveit { namespace task_constructor { namespace subtasks {
CartesianPositionMotion::CartesianPositionMotion(std::string name)
: PropagatingAnyWay(name),
: PropagatingEitherWay(name),
step_size_(0.005)
{
ros::NodeHandle nh;
@ -70,9 +70,8 @@ namespace {
}
}
bool CartesianPositionMotion::computeForward(const InterfaceState &beginning, planning_scene::PlanningScenePtr &result_scene,
robot_trajectory::RobotTrajectoryPtr &traj, double &cost){
result_scene = beginning.state->diff();
bool CartesianPositionMotion::computeForward(const InterfaceState &from){
planning_scene::PlanningScenePtr result_scene = from.state->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
@ -90,7 +89,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &beginning, pl
bool succeeded= false;
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
const Eigen::Affine3d& frame= beginning.state->getFrameTransform(towards_.header.frame_id);
const Eigen::Affine3d& frame= from.state->getFrameTransform(towards_.header.frame_id);
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_);
@ -144,21 +143,22 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &beginning, pl
if(succeeded){
traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
robot_trajectory::RobotTrajectoryPtr traj
= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
for( auto& tp : trajectory_steps )
traj->addSuffixWayPoint(tp, 0.0);
sendForward(from, result_scene, traj);
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state;
_publishTrajectory(*traj, *result_state);
_publishTrajectory(result_scene, *traj, *result_state);
}
return succeeded;
}
bool CartesianPositionMotion::computeBackward(planning_scene::PlanningScenePtr &result_scene, const InterfaceState &ending,
robot_trajectory::RobotTrajectoryPtr &traj, double &cost){
result_scene = ending.state->diff();
bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
planning_scene::PlanningScenePtr result_scene = to.state->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
@ -210,24 +210,27 @@ bool CartesianPositionMotion::computeBackward(planning_scene::PlanningScenePtr &
bool succeeded= achieved_distance >= min_distance_;
if(succeeded){
traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
for( auto& tp : trajectory_steps )
traj->addPrefixWayPoint(tp, 0.0);
sendBackward(result_scene, to, traj);
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state;
_publishTrajectory(*traj, *result_state);
_publishTrajectory(result_scene, *traj, *result_state);
}
return succeeded;
}
void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
void CartesianPositionMotion::_publishTrajectory(const planning_scene::PlanningSceneConstPtr& scene,
const robot_trajectory::RobotTrajectory& trajectory,
const moveit::core::RobotState& start){
moveit_msgs::DisplayTrajectory dt;
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
dt.model_id= scene()->getRobotModel()->getName();
dt.model_id= scene->getRobotModel()->getName();
dt.trajectory.resize(1);
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
dt.model_id= start.getRobotModel()->getName();

View File

@ -8,13 +8,19 @@ CurrentState::CurrentState(std::string name)
ran_= false;
}
bool CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
{
scene_ = scene;
ran_= false;
}
bool CurrentState::canCompute() const{
return !ran_;
}
bool CurrentState::compute(){
ran_= true;
spawn(scene());
spawn(scene_);
return true;
}

View File

@ -33,6 +33,11 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
ros::Duration(1.0).sleep();
}
bool GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
{
scene_ = scene;
}
void GenerateGraspPose::setGroup(std::string group){
group_= group;
}
@ -102,9 +107,9 @@ namespace {
}
bool GenerateGraspPose::compute(){
assert(scene()->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf");
assert(scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf");
planning_scene::PlanningScenePtr grasp_scene = scene()->diff();
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
@ -122,7 +127,7 @@ bool GenerateGraspPose::compute(){
const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind(
&isValid,
scene(),
scene_,
ignore_collisions_,
&previous_solutions_,
std::placeholders::_1,
@ -130,7 +135,7 @@ bool GenerateGraspPose::compute(){
std::placeholders::_3);
geometry_msgs::Pose object_pose, grasp_pose;
const Eigen::Affine3d object_pose_eigen= scene()->getFrameTransform(object_);
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_);
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
throw std::runtime_error("requested object does not exist or could not be retrieved");

View File

@ -1,5 +1,6 @@
#include <moveit_task_constructor/subtasks/gripper.h>
#include <moveit_task_constructor/storage.h>
#include <moveit_task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
@ -13,9 +14,14 @@
namespace moveit { namespace task_constructor { namespace subtasks {
Gripper::Gripper(std::string name)
: PropagatingAnyWay(name)
: PropagatingEitherWay(name)
{}
bool Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
{
planner_ = Task::createPlanner(scene->getRobotModel());
}
void Gripper::setEndEffector(std::string eef){
eef_= eef;
}
@ -64,7 +70,7 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
}
::planning_interface::MotionPlanResponse res;
if(!planner()->generatePlan(scene, req, res))
if(!planner_->generatePlan(scene, req, res))
return false;
trajectory = res.trajectory_;
@ -83,14 +89,27 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
return true;
}
bool Gripper::computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost){
return compute(from, to, trajectory, cost, FORWARD);
bool Gripper::computeForward(const InterfaceState &from){
planning_scene::PlanningScenePtr to;
robot_trajectory::RobotTrajectoryPtr trajectory;
double cost = 0;
if (!compute(from, to, trajectory, cost, FORWARD))
return false;
sendForward(from, to, trajectory, cost);
return true;
}
bool Gripper::computeBackward(planning_scene::PlanningScenePtr &from, const InterfaceState &to,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost)
bool Gripper::computeBackward(const InterfaceState &to)
{
return compute(to, from, trajectory, cost, BACKWARD);
planning_scene::PlanningScenePtr from;
robot_trajectory::RobotTrajectoryPtr trajectory;
double cost = 0;
if (!compute(to, from, trajectory, cost, BACKWARD))
return false;
sendBackward(from, to, trajectory, cost);
return true;
}
} } }

View File

@ -1,5 +1,6 @@
#include <moveit_task_constructor/subtasks/move.h>
#include <moveit_task_constructor/storage.h>
#include <moveit_task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
@ -17,15 +18,16 @@ Move::Move(std::string name)
timeout_(5.0)
{}
bool Move::init(const planning_scene::PlanningSceneConstPtr &scene)
{
planner_ = Task::createPlanner(scene->getRobotModel());
}
void Move::setGroup(std::string group){
group_= group;
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
}
void Move::setLink(std::string link){
link_= link;
}
void Move::setPlannerId(std::string planner){
planner_id_= planner;
}
@ -34,24 +36,8 @@ void Move::setTimeout(double timeout){
timeout_= timeout;
}
/* TODO: implement this in compute
void Move::setFrom(std::string named_target){
from_named_target_= named_target;
}
void Move::setTo(std::string named_target){
to_named_target_= named_target;
}
*/
bool Move::canCompute() const{
return hasStatePair();
}
bool Move::compute(){
InterfaceStatePair state_pair= fetchStatePair();
mgi_->setJointValueTarget(state_pair.second.state->getCurrentState());
bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
mgi_->setJointValueTarget(to.state->getCurrentState());
if( !planner_id_.empty() )
mgi_->setPlannerId(planner_id_);
mgi_->setPlanningTime(timeout_);
@ -61,11 +47,11 @@ bool Move::compute(){
ros::Duration(4.0).sleep();
::planning_interface::MotionPlanResponse res;
if(!planner()->generatePlan(state_pair.first.state, req, res))
if(!planner_->generatePlan(from.state, req, res))
return false;
// finish subtask
connect(res.trajectory_, state_pair);
connect(from, to, res.trajectory_);
return true;
}

View File

@ -16,14 +16,13 @@ namespace moveit { namespace task_constructor {
class TaskPrivate : public SerialContainerPrivate {
friend class Task;
robot_model_loader::RobotModelLoaderPtr rml_;
planning_scene::PlanningSceneConstPtr scene_;
public:
TaskPrivate(Task* me, const std::string &name)
: SerialContainerPrivate(me, name)
{
initModel();
initScene();
initPlanner();
}
void initModel () {
@ -60,11 +59,6 @@ public:
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
}
void initPlanner() {
assert(rml_);
assert(scene_);
planner_ = std::make_shared<planning_pipeline::PlanningPipeline>(rml_->getModel(), ros::NodeHandle("move_group"));
}
};
@ -73,6 +67,27 @@ Task::Task(const std::string &name)
{
}
planning_pipeline::PlanningPipelinePtr
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
const std::string& planning_plugin_param_name,
const std::string& adapter_plugins_param_name) {
typedef std::tuple<std::string, std::string, std::string, std::string> PlannerID;
static std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> > planner_cache;
PlannerID id (model->getName(), ns, planning_plugin_param_name, adapter_plugins_param_name);
auto it = planner_cache.find(id);
planning_pipeline::PlanningPipelinePtr planner;
if (it != planner_cache.cend())
planner = it->second.lock();
if (!planner) {
planner = std::make_shared<planning_pipeline::PlanningPipeline>
(model, ros::NodeHandle(ns), planning_plugin_param_name, adapter_plugins_param_name);
planner_cache[id] = planner;
}
return planner;
}
void Task::add(pointer &&stage) {
if (!stage)
throw std::runtime_error("stage insertion failed: invalid stage pointer");
@ -81,8 +96,11 @@ void Task::add(pointer &&stage) {
}
bool Task::plan(){
IMPL(Task);
NewSolutionPublisher debug(*this);
impl->initScene();
this->init(impl->scene_);
while(ros::ok() && canCompute()) {
if (compute()) {
debug.publish();
@ -94,7 +112,8 @@ bool Task::plan(){
}
const robot_state::RobotState& Task::getCurrentRobotState() const {
return pimpl_func()->scene_->getCurrentState();
IMPL(const Task)
return impl->scene_->getCurrentState();
}
void Task::printState(){
@ -135,7 +154,7 @@ bool traverseFullTrajectories(
bool Task::processSolutions(const Task::SolutionCallback& processor) {
IMPL(Task);
const TaskPrivate::array_type& children = impl->children();
const TaskPrivate::container_type& children = impl->children();
const size_t nr_of_trajectories = children.size();
std::vector<SubTrajectory*> trace;
trace.reserve(nr_of_trajectories);

View File

@ -29,8 +29,7 @@ public:
class TestPropagatingForward : public PropagatingForward {
public:
TestPropagatingForward() : PropagatingForward("forwarder") {}
bool canCompute() const override { return true; }
bool compute() override { return false; }
bool computeForward(const InterfaceState &from) override { return false; }
};
@ -79,7 +78,9 @@ protected:
TEST_F(BaseTest, interfaceFlags) {
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
EXPECT_EQ(g->interfaceFlags(), SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT, SubTask::WRITES_PREV_OUTPUT}));
EXPECT_EQ(g->pimpl_func()->interfaceFlags(),
SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START,
SubTaskPrivate::WRITES_PREV_END}));
}
#define VALIDATE(...) \
@ -90,8 +91,8 @@ TEST_F(BaseTest, serialContainer) {
SerialContainer c("serial");
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(c.pimpl_func());
EXPECT_TRUE(bool(cp->input_));
EXPECT_TRUE(bool(cp->output_));
EXPECT_TRUE(bool(cp->starts_));
EXPECT_TRUE(bool(cp->ends_));
EXPECT_EQ(parent(cp), nullptr);
EXPECT_EQ(parent(cp), nullptr);
VALIDATE();