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/debug.cpp
src/subtask_p.h src/subtask_p.h
src/container_p.h
include/${PROJECT_NAME}/utils.h include/${PROJECT_NAME}/utils.h
include/${PROJECT_NAME}/storage.h include/${PROJECT_NAME}/storage.h
include/${PROJECT_NAME}/subtask.h include/${PROJECT_NAME}/subtask.h

View File

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

View File

@ -42,41 +42,17 @@ public:
inline SubTaskPrivate* pimpl_func() { return pimpl_; } inline SubTaskPrivate* pimpl_func() { return pimpl_; }
typedef std::unique_ptr<SubTask> pointer; 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(); ~SubTask();
/// initialize stage once before planning
virtual bool init(const planning_scene::PlanningSceneConstPtr& scene);
const std::string& getName() const; 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: protected:
/// can only instantiated by derived classes /// SubTask can only be instantiated through derived classes
SubTask(SubTaskPrivate *impl); 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: protected:
// TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor // TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor
SubTaskPrivate* const pimpl_; // constness guarantees one initial write SubTaskPrivate* const pimpl_; // constness guarantees one initial write
@ -84,55 +60,57 @@ protected:
std::ostream& operator<<(std::ostream &os, const SubTask& stage); std::ostream& operator<<(std::ostream &os, const SubTask& stage);
class PropagatingAnyWayPrivate; class PropagatingEitherWayPrivate;
class PropagatingAnyWay : public SubTask { class PropagatingEitherWay : public SubTask {
public: public:
PRIVATE_CLASS(PropagatingAnyWay) PRIVATE_CLASS(PropagatingEitherWay)
PropagatingAnyWay(const std::string& name); PropagatingEitherWay(const std::string& name);
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 computeForward(const InterfaceState& from, planning_scene::PlanningScenePtr& to, virtual bool computeForward(const InterfaceState& from) = 0;
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost); virtual bool computeBackward(const InterfaceState& to) = 0;
virtual bool computeBackward(planning_scene::PlanningScenePtr& from, const InterfaceState& to,
robot_trajectory::RobotTrajectoryPtr& trajectory, double& cost);
bool canCompute() const override; void sendForward(const InterfaceState& from,
bool compute() override; 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: protected:
// constructor for use in derived classes // constructor for use in derived classes
PropagatingAnyWay(PropagatingAnyWayPrivate* impl); PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
void initInterface(); 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 PropagatingForwardPrivate;
class PropagatingForward : public PropagatingAnyWay { class PropagatingForward : public PropagatingEitherWay {
public: public:
PRIVATE_CLASS(PropagatingForward) PRIVATE_CLASS(PropagatingForward)
PropagatingForward(const std::string& name); PropagatingForward(const std::string& name);
private: private:
// restrict access to backward method to provide compile-time check // 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 PropagatingBackwardPrivate;
class PropagatingBackward : public PropagatingAnyWay { class PropagatingBackward : public PropagatingEitherWay {
public: public:
PRIVATE_CLASS(PropagatingBackward) PRIVATE_CLASS(PropagatingBackward)
PropagatingBackward(const std::string& name); PropagatingBackward(const std::string& name);
private: private:
// restrict access to forward method to provide compile-time check // 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) PRIVATE_CLASS(Generator)
Generator(const std::string& name); 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) PRIVATE_CLASS(Connecting)
Connecting(const std::string& name); Connecting(const std::string& name);
// methods for manual use virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
bool hasStatePair() const; void connect(const InterfaceState& from, const InterfaceState& to,
InterfaceStatePair fetchStatePair(); const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost = 0);
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);
}; };

View File

@ -16,14 +16,12 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
class CartesianPositionMotion : public PropagatingAnyWay { class CartesianPositionMotion : public PropagatingEitherWay {
public: public:
CartesianPositionMotion(std::string name); CartesianPositionMotion(std::string name);
virtual bool computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to, virtual bool computeForward(const InterfaceState &from) override;
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) override; virtual bool computeBackward(const InterfaceState &to) override;
virtual bool computeBackward(planning_scene::PlanningScenePtr &from, const InterfaceState &to,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) override;
void setGroup(std::string group); void setGroup(std::string group);
void setLink(std::string link); void setLink(std::string link);
@ -57,7 +55,7 @@ protected:
ros::Publisher pub; 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: public:
CurrentState(std::string name); CurrentState(std::string name);
virtual bool canCompute() const override; bool init(const planning_scene::PlanningSceneConstPtr& scene) override;
virtual bool compute() override; bool canCompute() const override;
bool compute() override;
protected: protected:
planning_scene::PlanningSceneConstPtr scene_;
bool ran_; bool ran_;
}; };

View File

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

View File

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

View File

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

View File

@ -1,4 +1,5 @@
// copyright Michael 'v4hn' Goerner @ 2017 // copyright Michael 'v4hn' Goerner @ 2017
// copyright Robert Haschke @ 2017
#pragma once #pragma once
@ -6,11 +7,8 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <ros/ros.h>
#include <vector>
namespace moveit { namespace core { namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(RobotState)
}} }}
@ -24,9 +22,11 @@ class TaskPrivate;
class Task : protected SerialContainer { class Task : protected SerialContainer {
PRIVATE_CLASS(Task) PRIVATE_CLASS(Task)
public: public:
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
Task(const std::string &name = std::string()); 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); void add(SubTask::pointer &&stage);
using SerialContainer::clear; using SerialContainer::clear;
@ -35,6 +35,8 @@ public:
void printState(); void printState();
const moveit::core::RobotState &getCurrentRobotState() const; 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);
bool processSolutions(const SolutionCallback &processor) const; 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) for (auto end = children_.end(); before > 0 && position != end; --before)
++position; ++position;
} else if (++before <= 0) { } 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) for (auto end = children_.rend(); before < 0 && from_end != end; ++before)
++from_end; ++from_end;
position = from_end.base(); position = from_end.base();
@ -29,9 +29,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
for (auto &stage : children_) { for (auto &stage : children_) {
if (!processor(*stage, depth)) if (!processor(*stage, depth))
continue; continue;
ContainerBase *container = dynamic_cast<ContainerBase*>(stage.get()); ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl_func());
if (container) if (container)
static_cast<ContainerBasePrivate*>(container->pimpl_func())->traverseStages(processor, depth+1); container->traverseStages(processor, depth+1);
} }
return true; return true;
} }
@ -40,8 +40,6 @@ ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate
ContainerBasePrivate::const_iterator pos) { ContainerBasePrivate::const_iterator pos) {
SubTaskPrivate *impl = subtask->pimpl_func(); SubTaskPrivate *impl = subtask->pimpl_func();
impl->parent_ = this; impl->parent_ = this;
subtask->setPlanningScene(scene_);
subtask->setPlanningPipeline(planner_);
impl->it_ = children_.insert(pos, std::move(subtask)); impl->it_ = children_.insert(pos, std::move(subtask));
return impl->it_; return impl->it_;
} }
@ -58,28 +56,46 @@ void ContainerBase::clear()
impl->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 bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
{ {
IMPL(const ContainerBase); IMPL(const ContainerBase);
return impl->traverseStages(processor, 0); return impl->traverseStages(processor, 0);
} }
bool ContainerBase::canCompute() const
{
IMPL(const ContainerBase);
return impl->canCompute();
}
SubTask::InterfaceFlags SerialContainerPrivate::announcedFlags() const { bool ContainerBase::compute() {
SubTask::InterfaceFlags f; IMPL(ContainerBase);
return impl->compute();
}
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
InterfaceFlags f;
if (children().empty()) return f; if (children().empty()) return f;
f |= children().front()->pimpl_func()->announcedFlags() & SubTask::INPUT_IF_MASK; f |= children().front()->pimpl_func()->announcedFlags() & INPUT_IF_MASK;
f |= children().back()->pimpl_func()->announcedFlags() & SubTask::OUTPUT_IF_MASK; f |= children().back()->pimpl_func()->announcedFlags() & OUTPUT_IF_MASK;
return f; return f;
} }
inline bool isConnectable(int prev_flags, int next_flags) { inline bool isConnectable(int prev_flags, int next_flags) {
return ((prev_flags & SubTask::WRITES_NEXT_INPUT) && (next_flags & SubTask::READS_INPUT)) || return ((prev_flags & SubTaskPrivate::WRITES_NEXT_START) && (next_flags & SubTaskPrivate::READS_START)) ||
((prev_flags & SubTask::READS_OUTPUT) && (next_flags & SubTask::WRITES_PREV_OUTPUT)); ((prev_flags & SubTaskPrivate::READS_END) && (next_flags & SubTaskPrivate::WRITES_PREV_END));
} }
inline bool bothWrite(SubTask::InterfaceFlags prev_flags, SubTask::InterfaceFlags next_flags) { inline bool bothWrite(SubTaskPrivate::InterfaceFlags prev_flags, SubTaskPrivate::InterfaceFlags next_flags) {
return (prev_flags.testFlag(SubTask::WRITES_NEXT_INPUT) && !next_flags.testFlag(SubTask::READS_INPUT)) && return (prev_flags.testFlag(SubTaskPrivate::WRITES_NEXT_START) && !next_flags.testFlag(SubTaskPrivate::READS_START)) &&
(next_flags.testFlag(SubTask::WRITES_PREV_OUTPUT) && !prev_flags.testFlag(SubTask::READS_OUTPUT)); (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 { 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 // check connectedness
bool at_end = (before == children().end()); bool at_end = (before == children().end());
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl_func(); const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl_func();
SubTask::InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags(); InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags();
SubTask::InterfaceFlags next_flags = next->deducedFlags(); InterfaceFlags next_flags = next->deducedFlags();
SubTask::InterfaceFlags prev_flags = prev(before)->deducedFlags(); InterfaceFlags prev_flags = prev(before)->deducedFlags();
// Do a simple check here only. A full connectivity check requires the full pipeline to be setup // 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 // 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()); bool at_end = (before == children().end());
SubTaskPrivate *cur = stage->pimpl_func(); 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 if (children().empty()) { // first child inserted
setPrevOutput(cur, this->input_); setPrevEnds(cur, this->starts_);
setNextInput(cur, this->output_); setNextStarts(cur, this->ends_);
} else if (at_begin) { } else if (at_begin) {
SubTaskPrivate *next = (*before)->pimpl_func(); SubTaskPrivate *next = (*before)->pimpl_func();
setPrevOutput(cur, this->input_); setPrevEnds(cur, this->starts_);
setNextInput(cur, next->input_); setNextStarts(cur, next->starts_);
setPrevOutput(next, cur->output_); setPrevEnds(next, cur->ends_);
} else if (at_end) { } else if (at_end) {
const SubTaskPrivate *prev = this->prev(before); const SubTaskPrivate *prev = this->prev(before);
setNextInput(prev, cur->input_); setNextStarts(prev, cur->starts_);
setPrevOutput(cur, prev->output_); setPrevEnds(cur, prev->ends_);
setNextInput(cur, this->output_); setNextStarts(cur, this->ends_);
} else { } else {
const SubTaskPrivate *prev = this->prev(before); const SubTaskPrivate *prev = this->prev(before);
SubTaskPrivate *next = (*before)->pimpl_func(); SubTaskPrivate *next = (*before)->pimpl_func();
setNextInput(prev, cur->input_); setNextStarts(prev, cur->starts_);
setPrevOutput(cur, prev->output_); setPrevEnds(cur, prev->ends_);
setNextInput(cur, next->input_); setNextStarts(cur, next->starts_);
setPrevOutput(next, cur->output_); setPrevEnds(next, cur->ends_);
} }
iterator it = ContainerBasePrivate::insert(std::move(stage), before); iterator it = ContainerBasePrivate::insert(std::move(stage), before);
@ -195,21 +211,19 @@ bool SerialContainer::insert(value_type&& subtask, int before)
return true; return true;
} }
bool SerialContainer::canCompute() const bool SerialContainerPrivate::canCompute() const
{ {
IMPL(const SerialContainer); return children().size() > 0;
return impl->children().size() > 0;
} }
bool SerialContainer::compute() bool SerialContainerPrivate::compute()
{ {
IMPL(SerialContainer);
bool computed = false; bool computed = false;
for(const auto& stage : impl->children()){ for(const auto& stage : children()) {
if(!stage->canCompute()) if(!stage->pimpl_func()->canCompute())
continue; continue;
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl; std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;
bool success = stage->compute(); bool success = stage->pimpl_func()->compute();
computed = true; computed = true;
std::cout << (success ? "succeeded" : "failed") << std::endl; std::cout << (success ? "succeeded" : "failed") << std::endl;
} }

View File

@ -7,14 +7,16 @@ namespace moveit { namespace task_constructor {
class ContainerBasePrivate : public SubTaskPrivate class ContainerBasePrivate : public SubTaskPrivate
{ {
friend class ContainerBase;
friend class BaseTest; // allow access for unit tests friend class BaseTest; // allow access for unit tests
public: public:
typedef ContainerBase::value_type value_type; typedef ContainerBase::value_type value_type;
typedef SubTaskPrivate::array_type array_type; typedef SubTaskPrivate::container_type container_type;
typedef array_type::iterator iterator; typedef container_type::iterator iterator;
typedef array_type::const_iterator const_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; const_iterator position(int before = -1) const;
bool canInsert(const SubTask& stage) const; bool canInsert(const SubTask& stage) const;
@ -30,15 +32,15 @@ protected:
inline const ContainerBasePrivate* parent(const SubTaskPrivate *child) const { return child->parent_; } inline const ContainerBasePrivate* parent(const SubTaskPrivate *child) const { return child->parent_; }
inline iterator it(const SubTaskPrivate *child) const { return child->it_; } inline iterator it(const SubTaskPrivate *child) const { return child->it_; }
inline void setPrevOutput(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) { inline void setPrevEnds(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->prev_output_ = interface.get(); child->prev_ends_ = interface.get();
} }
inline void setNextInput(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) { inline void setNextStarts(const SubTaskPrivate* child, const InterfacePtr& interface = InterfacePtr()) {
child->next_input_ = interface.get(); child->next_starts_ = interface.get();
} }
private: private:
array_type children_; container_type children_;
}; };
@ -47,14 +49,17 @@ public:
SerialContainerPrivate(SerialContainer* me, const std::string &name) SerialContainerPrivate(SerialContainer* me, const std::string &name)
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
{ {
input_.reset(new Interface(Interface::NotifyFunction())); starts_.reset(new Interface(Interface::NotifyFunction()));
output_.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; bool canInsert(const SubTask& stage, const_iterator before) const;
virtual iterator insert(value_type &&stage, const_iterator before) override; 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 *prev(const_iterator it) const;
inline const SubTaskPrivate *next(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"); auto move= std::make_unique<subtasks::Move>("move to pre-grasp");
move->setGroup("left_arm"); move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame");
move->setPlannerId("RRTConnectkConfigDefault"); move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0); move->setTimeout(8.0);
t.add(std::move(move)); 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() SubTask::~SubTask()
{ {
delete pimpl_; delete pimpl_;
} }
bool SubTask::init(const planning_scene::PlanningSceneConstPtr &scene)
{
}
const std::string& SubTask::getName() const { const std::string& SubTask::getName() const {
return pimpl_->name_; return pimpl_->name_;
} }
@ -33,12 +29,12 @@ std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
return os; return os;
} }
template<SubTask::InterfaceFlag own, SubTask::InterfaceFlag other> template<SubTaskPrivate::InterfaceFlag own, SubTaskPrivate::InterfaceFlag other>
const char* direction(const SubTaskPrivate& stage) { const char* direction(const SubTaskPrivate& stage) {
SubTask::InterfaceFlags f = stage.deducedFlags(); SubTaskPrivate::InterfaceFlags f = stage.deducedFlags();
bool own_if = f & own; bool own_if = f & own;
bool other_if = f & other; 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 (!own_if && !other_if) return "--"; if (!own_if && !other_if) return "--";
if (other_if ^ reverse) 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) { std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
// inputs // starts
for (const Interface* i : {stage.prev_output_, stage.input_.get()}) { for (const Interface* i : {stage.prev_ends_, stage.starts_.get()}) {
os << std::setw(3); os << std::setw(3);
if (i) os << i->size(); if (i) os << i->size();
else os << "-"; else os << "-";
} }
// trajectories // 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(3) << stage.trajectories_.size()
<< std::setw(5) << direction<SubTask::READS_OUTPUT, SubTask::WRITES_NEXT_INPUT>(stage); << std::setw(5) << direction<SubTaskPrivate::READS_END, SubTaskPrivate::WRITES_NEXT_START>(stage);
// outputs // ends
for (const Interface* i : {stage.output_.get(), stage.next_input_}) { for (const Interface* i : {stage.ends_.get(), stage.next_starts_}) {
os << std::setw(3); os << std::setw(3);
if (i) os << i->size(); if (i) os << i->size();
else os << "-"; else os << "-";
@ -67,164 +63,162 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
return os; return os;
} }
planning_scene::PlanningSceneConstPtr SubTask::scene() const
{
return pimpl_->scene_;
}
planning_pipeline::PlanningPipelinePtr SubTask::planner() const SubTaskPrivate::SubTaskPrivate(SubTask *me, const std::string &name)
{ : me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr)
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)
{} {}
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
trajectories_.emplace_back(trajectory); 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 // 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; InterfaceFlags f;
if (input_) f |= SubTask::READS_INPUT; if (starts_) f |= READS_START;
if (output_) f |= SubTask::READS_OUTPUT; if (ends_) f |= READS_END;
if (prevOutput()) f |= SubTask::WRITES_PREV_OUTPUT; if (prevEnds()) f |= WRITES_PREV_END;
if (nextInput()) f |= SubTask::WRITES_NEXT_INPUT; if (nextStarts()) f |= WRITES_NEXT_START;
return f; 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){ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
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)
: SubTaskPrivate(me, name), dir(dir) : SubTaskPrivate(me, name), dir(dir)
{ {
} }
SubTask::InterfaceFlags PropagatingAnyWayPrivate::announcedFlags() const { SubTaskPrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
SubTask::InterfaceFlags f; InterfaceFlags f;
if (dir & PropagatingAnyWay::FORWARD) if (dir & PropagatingEitherWay::FORWARD)
f |= SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::WRITES_NEXT_INPUT}); f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
if (dir & PropagatingAnyWay::BACKWARD) if (dir & PropagatingEitherWay::BACKWARD)
f |= SubTask::InterfaceFlags({SubTask::READS_OUTPUT, SubTask::WRITES_PREV_OUTPUT}); f |= InterfaceFlags({READS_END, WRITES_PREV_END});
return f; return f;
} }
inline bool PropagatingAnyWayPrivate::hasStartState() const{ inline bool PropagatingEitherWayPrivate::hasStartState() const{
return next_input_state_ != input_->end(); return next_start_state_ != starts_->end();
} }
const InterfaceState& PropagatingAnyWayPrivate::fetchStartState(){ const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
if (!hasStartState()) if (!hasStartState())
throw std::runtime_error("no new state for beginning available"); throw std::runtime_error("no new state for beginning available");
const InterfaceState& state= *next_input_state_; const InterfaceState& state= *next_start_state_;
++next_input_state_; ++next_start_state_;
return state; return state;
} }
bool PropagatingAnyWayPrivate::sendForward(const robot_trajectory::RobotTrajectoryPtr& t, inline bool PropagatingEitherWayPrivate::hasEndState() const{
const InterfaceState& from, return next_end_state_ != ends_->end();
const planning_scene::PlanningSceneConstPtr& to,
double cost){
SubTrajectory &trajectory = addTrajectory(t, cost);
trajectory.setStartState(from);
return SubTaskPrivate::sendForward(trajectory, to);
} }
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
inline bool PropagatingAnyWayPrivate::hasEndState() const{
return next_output_state_ != output_->end();
}
const InterfaceState& PropagatingAnyWayPrivate::fetchEndState(){
if(!hasEndState()) if(!hasEndState())
throw std::runtime_error("no new state for ending available"); throw std::runtime_error("no new state for ending available");
const InterfaceState& state= *next_output_state_; const InterfaceState& state= *next_end_state_;
++next_output_state_; ++next_end_state_;
return state; return state;
} }
bool PropagatingAnyWayPrivate::sendBackward(const robot_trajectory::RobotTrajectoryPtr& t, bool PropagatingEitherWayPrivate::canCompute() const
const planning_scene::PlanningSceneConstPtr& from, {
const InterfaceState& to, if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
double cost){ return true;
SubTrajectory& trajectory = addTrajectory(t, cost); if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState())
trajectory.setEndState(to); return true;
return SubTaskPrivate::sendBackward(trajectory, from); 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) PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
: PropagatingAnyWay(new PropagatingAnyWayPrivate(this, ANYWAY, name)) : PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
{ {
} }
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
: SubTask(impl) : SubTask(impl)
{ {
initInterface(); initInterface();
} }
void PropagatingAnyWay::initInterface() void PropagatingEitherWay::initInterface()
{ {
IMPL(PropagatingAnyWay) IMPL(PropagatingEitherWay)
if (impl->dir & PropagatingAnyWay::FORWARD) { if (impl->dir & PropagatingEitherWay::FORWARD) {
if (!impl->input_) { // keep existing interface if possible if (!impl->starts_) { // keep existing interface if possible
impl->input_.reset(new Interface([this](const Interface::iterator& it) { newInputState(it); })); impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); }));
impl->next_input_state_ = impl->input_->begin(); impl->next_start_state_ = impl->starts_->begin();
} }
} else { } else {
impl->input_.reset(); impl->starts_.reset();
impl->next_input_state_ = Interface::iterator(); impl->next_start_state_ = Interface::iterator();
} }
if (impl->dir & PropagatingAnyWay::BACKWARD) { if (impl->dir & PropagatingEitherWay::BACKWARD) {
if (!impl->output_) { // keep existing interface if possible if (!impl->ends_) { // keep existing interface if possible
impl->output_.reset(new Interface([this](const Interface::iterator& it) { newOutputState(it); })); impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); }));
impl->next_output_state_ = impl->output_->end(); impl->next_end_state_ = impl->ends_->end();
} }
} else { } else {
impl->output_.reset(); impl->ends_.reset();
impl->next_output_state_ = Interface::iterator(); 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->dir == dir) return;
if (impl->isConnected()) if (impl->isConnected())
throw std::runtime_error("Cannot change direction after being connected"); throw std::runtime_error("Cannot change direction after being connected");
@ -232,117 +226,81 @@ void PropagatingAnyWay::restrictDirection(PropagatingAnyWay::Direction dir)
initInterface(); initInterface();
} }
bool PropagatingAnyWay::computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to, void PropagatingEitherWay::sendForward(const InterfaceState& from,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) const planning_scene::PlanningSceneConstPtr& to,
{ const robot_trajectory::RobotTrajectoryPtr& t,
// default implementation is void, needs override by user double cost){
return false; 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, void PropagatingEitherWay::sendBackward(const planning_scene::PlanningSceneConstPtr& from,
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost) const InterfaceState& to,
{ const robot_trajectory::RobotTrajectoryPtr& t,
// default implementation is void, needs override by user double cost){
return false; IMPL(PropagatingEitherWay)
} std::cout << "sending state backward" << std::endl;
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
bool PropagatingAnyWay::canCompute() const trajectory.setEndState(to);
{ impl->prevEnds()->add(from, NULL, &trajectory);
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_;
} }
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, const std::__cxx11::string &name) PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, const std::string &name)
: PropagatingAnyWayPrivate(me, PropagatingAnyWay::FORWARD, name) : PropagatingEitherWayPrivate(me, PropagatingEitherWay::FORWARD, name)
{ {
// indicate, that we don't accept new states from output interface // indicate, that we don't accept new states from ends_ interface
output_.reset(); ends_.reset();
next_output_state_ = Interface::iterator(); next_end_state_ = Interface::iterator();
} }
PropagatingForward::PropagatingForward(const std::string& name) PropagatingForward::PropagatingForward(const std::string& name)
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name)) : PropagatingEitherWay(new PropagatingForwardPrivate(this, name))
{} {}
bool PropagatingForward::computeBackward(const InterfaceState &to)
PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, const std::__cxx11::string &name)
: PropagatingAnyWayPrivate(me, PropagatingAnyWay::BACKWARD, name)
{ {
// indicate, that we don't accept new states from input interface assert(false); // This should never be called
input_.reset(); }
next_input_state_ = Interface::iterator();
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) 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) : SubTaskPrivate(me, name)
{} {}
SubTask::InterfaceFlags GeneratorPrivate::announcedFlags() const { SubTaskPrivate::InterfaceFlags GeneratorPrivate::announcedFlags() const {
return SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT,SubTask::WRITES_PREV_OUTPUT}); return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END});
} }
inline bool GeneratorPrivate::spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost) bool GeneratorPrivate::canCompute() const {
{ return static_cast<Generator*>(me_)->canCompute();
// empty trajectory ref -> this node only produces states }
robot_trajectory::RobotTrajectoryPtr dummy;
SubTrajectory& trajectory = addTrajectory(dummy, cost);
std::cout << "spawning state" << std::endl; bool GeneratorPrivate::compute() {
bool result = sendBackward(trajectory, ps); return static_cast<Generator*>(me_)->compute();
result |= sendForward(trajectory, ps);
return result;
} }
@ -350,30 +308,55 @@ Generator::Generator(const std::string &name)
: SubTask(new GeneratorPrivate(this, 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) 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) : SubTaskPrivate(me, name)
{ {
input_.reset(new Interface(std::bind(&Connecting::newInputState, me, std::placeholders::_1))); starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
output_.reset(new Interface(std::bind(&Connecting::newOutputState, me, std::placeholders::_1))); ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
it_pairs_ = std::make_pair(input_->begin(), output_->begin()); it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
} }
SubTask::InterfaceFlags ConnectingPrivate::announcedFlags() const { SubTaskPrivate::InterfaceFlags ConnectingPrivate::announcedFlags() const {
return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::READS_OUTPUT}); 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); // TODO: need to handle the pairs iterator
trajectory.setStartState(state_pair.first); if(it_pairs_.first == starts_->end())
trajectory.setEndState(state_pair.second); --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) void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
{ const robot_trajectory::RobotTrajectoryPtr& t, double cost) {
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)
{
IMPL(Connecting) 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); friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
public: public:
typedef std::list<SubTask::pointer> array_type; typedef std::list<SubTask::pointer> container_type;
SubTaskPrivate(SubTask* me, const std::string& name); SubTaskPrivate(SubTask* me, const std::string& name);
SubTask::InterfaceFlags deducedFlags() const; enum InterfaceFlag {
virtual SubTask::InterfaceFlags announcedFlags() const = 0; 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_; } std::list<SubTrajectory>& trajectories() { return trajectories_; }
virtual bool canCompute() const = 0;
virtual bool compute() = 0;
public: public:
SubTask* const me_; // associated/owning SubTask instance SubTask* const me_; // associated/owning SubTask instance
const std::string name_; const std::string name_;
planning_scene::PlanningSceneConstPtr scene_; InterfacePtr starts_;
planning_pipeline::PlanningPipelinePtr planner_; InterfacePtr ends_;
InterfacePtr input_; inline ContainerBasePrivate* parent() const { return parent_; }
InterfacePtr output_; inline bool isConnected() const { return prev_ends_ || next_starts_; }
inline Interface* prevEnds() const { return prev_ends_; }
inline const Interface* prevOutput() const { return prev_output_; } inline Interface* nextStarts() const { return next_starts_; }
inline const Interface* nextInput() const { return next_input_; } SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
inline bool isConnected() const { return prev_output_ || next_input_; }
protected: protected:
std::list<SubTrajectory> trajectories_; 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: private:
// !! items accessed only by ContainerBasePrivate to maintain hierarchy !! // !! items accessed only by ContainerBasePrivate to maintain hierarchy !!
ContainerBasePrivate* parent_; // owning parent ContainerBasePrivate* parent_; // owning parent
array_type::iterator it_; // iterator into parent's children_ list referring to this container_type::iterator it_; // iterator into parent's children_ list referring to this
// caching the pointers to the output_ / input_ interface of previous / next stage // caching the pointers to the ends_ / starts_ interface of previous / next stage
mutable Interface *prev_output_; // interface to be used for sendBackward() mutable Interface *prev_ends_; // interface to be used for sendBackward()
mutable Interface *next_input_; // interface to be use for sendForward() mutable Interface *next_starts_; // interface to be use for sendForward()
}; };
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
class PropagatingAnyWayPrivate : public SubTaskPrivate { class PropagatingEitherWayPrivate : public SubTaskPrivate {
friend class PropagatingAnyWay; friend class PropagatingEitherWay;
public: 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); const std::string &name);
SubTask::InterfaceFlags announcedFlags() const override; InterfaceFlags announcedFlags() const override;
bool canCompute() const override;
bool compute() override;
bool hasStartState() const; bool hasStartState() const;
const InterfaceState &fetchStartState(); const InterfaceState &fetchStartState();
bool sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to,
double cost = 0);
bool hasEndState() const; bool hasEndState() const;
const InterfaceState &fetchEndState(); const InterfaceState &fetchEndState();
bool sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to,
double cost = 0);
protected: protected:
Interface::const_iterator next_input_state_; // get informed when new start or end state becomes available
Interface::const_iterator next_output_state_; 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: public:
inline PropagatingForwardPrivate(PropagatingForward *me, const std::string &name); 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: public:
inline PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name); 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 { class GeneratorPrivate : public SubTaskPrivate {
public: public:
inline GeneratorPrivate(Generator *me, const std::string &name); inline GeneratorPrivate(Generator *me, const std::string &name);
SubTask::InterfaceFlags announcedFlags() const override; InterfaceFlags announcedFlags() const override;
bool spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost);
bool canCompute() const override;
bool compute() override;
}; };
@ -121,11 +123,19 @@ class ConnectingPrivate : public SubTaskPrivate {
public: public:
inline ConnectingPrivate(Connecting *me, const std::string &name); 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, void connect(const robot_trajectory::RobotTrajectoryPtr& t,
const InterfaceStatePair& state_pair, double cost); const InterfaceStatePair& state_pair, double cost);
private: 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_; std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
}; };

View File

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

View File

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

View File

@ -33,6 +33,11 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
ros::Duration(1.0).sleep(); ros::Duration(1.0).sleep();
} }
bool GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
{
scene_ = scene;
}
void GenerateGraspPose::setGroup(std::string group){ void GenerateGraspPose::setGroup(std::string group){
group_= group; group_= group;
} }
@ -102,9 +107,9 @@ namespace {
} }
bool GenerateGraspPose::compute(){ 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(); robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_); const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
@ -122,7 +127,7 @@ bool GenerateGraspPose::compute(){
const moveit::core::GroupStateValidityCallbackFn is_valid= const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind( std::bind(
&isValid, &isValid,
scene(), scene_,
ignore_collisions_, ignore_collisions_,
&previous_solutions_, &previous_solutions_,
std::placeholders::_1, std::placeholders::_1,
@ -130,7 +135,7 @@ bool GenerateGraspPose::compute(){
std::placeholders::_3); std::placeholders::_3);
geometry_msgs::Pose object_pose, grasp_pose; 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()) 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"); 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/subtasks/gripper.h>
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
#include <moveit_task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
@ -13,9 +14,14 @@
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
Gripper::Gripper(std::string name) 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){ void Gripper::setEndEffector(std::string eef){
eef_= eef; eef_= eef;
} }
@ -64,7 +70,7 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
} }
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;
if(!planner()->generatePlan(scene, req, res)) if(!planner_->generatePlan(scene, req, res))
return false; return false;
trajectory = res.trajectory_; trajectory = res.trajectory_;
@ -83,14 +89,27 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
return true; return true;
} }
bool Gripper::computeForward(const InterfaceState &from, planning_scene::PlanningScenePtr &to, bool Gripper::computeForward(const InterfaceState &from){
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost){ planning_scene::PlanningScenePtr to;
return compute(from, to, trajectory, cost, FORWARD); 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/subtasks/move.h>
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
#include <moveit_task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
@ -17,15 +18,16 @@ Move::Move(std::string name)
timeout_(5.0) timeout_(5.0)
{} {}
bool Move::init(const planning_scene::PlanningSceneConstPtr &scene)
{
planner_ = Task::createPlanner(scene->getRobotModel());
}
void Move::setGroup(std::string group){ void Move::setGroup(std::string group){
group_= group; group_= group;
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_); mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
} }
void Move::setLink(std::string link){
link_= link;
}
void Move::setPlannerId(std::string planner){ void Move::setPlannerId(std::string planner){
planner_id_= planner; planner_id_= planner;
} }
@ -34,24 +36,8 @@ void Move::setTimeout(double timeout){
timeout_= timeout; timeout_= timeout;
} }
/* TODO: implement this in compute bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
void Move::setFrom(std::string named_target){ mgi_->setJointValueTarget(to.state->getCurrentState());
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());
if( !planner_id_.empty() ) if( !planner_id_.empty() )
mgi_->setPlannerId(planner_id_); mgi_->setPlannerId(planner_id_);
mgi_->setPlanningTime(timeout_); mgi_->setPlanningTime(timeout_);
@ -61,11 +47,11 @@ bool Move::compute(){
ros::Duration(4.0).sleep(); ros::Duration(4.0).sleep();
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;
if(!planner()->generatePlan(state_pair.first.state, req, res)) if(!planner_->generatePlan(from.state, req, res))
return false; return false;
// finish subtask // finish subtask
connect(res.trajectory_, state_pair); connect(from, to, res.trajectory_);
return true; return true;
} }

View File

@ -16,14 +16,13 @@ namespace moveit { namespace task_constructor {
class TaskPrivate : public SerialContainerPrivate { class TaskPrivate : public SerialContainerPrivate {
friend class Task; friend class Task;
robot_model_loader::RobotModelLoaderPtr rml_; robot_model_loader::RobotModelLoaderPtr rml_;
planning_scene::PlanningSceneConstPtr scene_;
public: public:
TaskPrivate(Task* me, const std::string &name) TaskPrivate(Task* me, const std::string &name)
: SerialContainerPrivate(me, name) : SerialContainerPrivate(me, name)
{ {
initModel(); initModel();
initScene();
initPlanner();
} }
void initModel () { void initModel () {
@ -60,11 +59,6 @@ public:
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel()); scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene); 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) { 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");
@ -81,8 +96,11 @@ void Task::add(pointer &&stage) {
} }
bool Task::plan(){ bool Task::plan(){
IMPL(Task);
NewSolutionPublisher debug(*this); NewSolutionPublisher debug(*this);
impl->initScene();
this->init(impl->scene_);
while(ros::ok() && canCompute()) { while(ros::ok() && canCompute()) {
if (compute()) { if (compute()) {
debug.publish(); debug.publish();
@ -94,7 +112,8 @@ bool Task::plan(){
} }
const robot_state::RobotState& Task::getCurrentRobotState() const { const robot_state::RobotState& Task::getCurrentRobotState() const {
return pimpl_func()->scene_->getCurrentState(); IMPL(const Task)
return impl->scene_->getCurrentState();
} }
void Task::printState(){ void Task::printState(){
@ -135,7 +154,7 @@ bool traverseFullTrajectories(
bool Task::processSolutions(const Task::SolutionCallback& processor) { bool Task::processSolutions(const Task::SolutionCallback& processor) {
IMPL(Task); IMPL(Task);
const TaskPrivate::array_type& children = impl->children(); const TaskPrivate::container_type& children = impl->children();
const size_t nr_of_trajectories = children.size(); const size_t nr_of_trajectories = children.size();
std::vector<SubTrajectory*> trace; std::vector<SubTrajectory*> trace;
trace.reserve(nr_of_trajectories); trace.reserve(nr_of_trajectories);

View File

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