diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b86d014..e9839001 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index 4cf36af2..3034d299 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -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); }; diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index 41777581..8bec53fa 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -42,41 +42,17 @@ public: inline SubTaskPrivate* pimpl_func() { return pimpl_; } typedef std::unique_ptr 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 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::iterator&) {} - virtual void newOutputState(const std::list::iterator&) {} - protected: // TODO: use unique_ptr 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::iterator& it); - void newOutputState(const std::list::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::iterator& it); - virtual void newOutputState(const std::list::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); }; diff --git a/include/moveit_task_constructor/subtasks/cartesian_position_motion.h b/include/moveit_task_constructor/subtasks/cartesian_position_motion.h index 6be260f3..42c096af 100644 --- a/include/moveit_task_constructor/subtasks/cartesian_position_motion.h +++ b/include/moveit_task_constructor/subtasks/cartesian_position_motion.h @@ -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); }; } } } diff --git a/include/moveit_task_constructor/subtasks/current_state.h b/include/moveit_task_constructor/subtasks/current_state.h index a055154e..703f1a93 100644 --- a/include/moveit_task_constructor/subtasks/current_state.h +++ b/include/moveit_task_constructor/subtasks/current_state.h @@ -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_; }; diff --git a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h index c1ac7301..5a7d6283 100644 --- a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h +++ b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h @@ -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_; diff --git a/include/moveit_task_constructor/subtasks/gripper.h b/include/moveit_task_constructor/subtasks/gripper.h index 9fc6a368..af290590 100644 --- a/include/moveit_task_constructor/subtasks/gripper.h +++ b/include/moveit_task_constructor/subtasks/gripper.h @@ -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_; }; diff --git a/include/moveit_task_constructor/subtasks/move.h b/include/moveit_task_constructor/subtasks/move.h index 0b0678cf..c5c9e5f3 100644 --- a/include/moveit_task_constructor/subtasks/move.h +++ b/include/moveit_task_constructor/subtasks/move.h @@ -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_; }; diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 4b0c3c2a..aabc2b1f 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -1,4 +1,5 @@ // copyright Michael 'v4hn' Goerner @ 2017 +// copyright Robert Haschke @ 2017 #pragma once @@ -6,11 +7,8 @@ #include -#include - -#include - 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&)> 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&)> SolutionCallback; bool processSolutions(const SolutionCallback &processor); bool processSolutions(const SolutionCallback &processor) const; }; diff --git a/src/container.cpp b/src/container.cpp index 8d888a19..e35f2e5b 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -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(stage.get()); + ContainerBasePrivate *container = dynamic_cast(stage->pimpl_func()); if (container) - static_cast(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; } diff --git a/src/container_p.h b/src/container_p.h index fd29de53..4b7e3328 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -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; diff --git a/src/demo/plan_pick_trixi.cpp b/src/demo/plan_pick_trixi.cpp index 911a9985..ac42c3f3 100644 --- a/src/demo/plan_pick_trixi.cpp +++ b/src/demo/plan_pick_trixi.cpp @@ -55,7 +55,6 @@ int main(int argc, char** argv){ { auto move= std::make_unique("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)); diff --git a/src/subtask.cpp b/src/subtask.cpp index d76fd300..071dd9c2 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -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 +template 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(stage) + os << std::setw(5) << direction(stage) << std::setw(3) << stage.trajectories_.size() - << std::setw(5) << direction(stage); - // outputs - for (const Interface* i : {stage.output_.get(), stage.next_input_}) { + << std::setw(5) << direction(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(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(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(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(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 - (*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); } } } diff --git a/src/subtask_p.h b/src/subtask_p.h index 3ccdbc47..e8e5e4e7 100644 --- a/src/subtask_p.h +++ b/src/subtask_p.h @@ -15,104 +15,106 @@ class SubTaskPrivate { friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); public: - typedef std::list array_type; + typedef std::list 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 InterfaceFlags; + + InterfaceFlags interfaceFlags() const; + InterfaceFlags deducedFlags() const; + virtual InterfaceFlags announcedFlags() const = 0; std::list& 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 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::iterator& it); + void newEndState(const std::list::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::iterator& it); + void newEndState(const std::list::iterator& it); + std::pair it_pairs_; }; diff --git a/src/subtasks/cartesian_position_motion.cpp b/src/subtasks/cartesian_position_motion.cpp index 9863c4e1..53cf3c61 100644 --- a/src/subtasks/cartesian_position_motion.cpp +++ b/src/subtasks/cartesian_position_motion.cpp @@ -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_state.getRobotModel(), jmg); + robot_trajectory::RobotTrajectoryPtr traj + = std::make_shared(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_state.getRobotModel(), jmg); + robot_trajectory::RobotTrajectoryPtr traj= std::make_shared(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(); diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp index dc4dd423..b56aa569 100644 --- a/src/subtasks/current_state.cpp +++ b/src/subtasks/current_state.cpp @@ -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; } diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index c9475287..f3ace218 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -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"); diff --git a/src/subtasks/gripper.cpp b/src/subtasks/gripper.cpp index b39d6f4a..3bfc3e00 100644 --- a/src/subtasks/gripper.cpp +++ b/src/subtasks/gripper.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -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; } } } } diff --git a/src/subtasks/move.cpp b/src/subtasks/move.cpp index 84400db1..e8eb033e 100644 --- a/src/subtasks/move.cpp +++ b/src/subtasks/move.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -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(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; } diff --git a/src/task.cpp b/src/task.cpp index 18e94099..97bc7530 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -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(rml_->getModel()); std::const_pointer_cast(scene_)->setPlanningSceneMsg(res.scene); } - void initPlanner() { - assert(rml_); - assert(scene_); - planner_ = std::make_shared(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 PlannerID; + static std::map > 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 + (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 trace; trace.reserve(nr_of_trajectories); diff --git a/src/test/container.cpp b/src/test/container.cpp index 1727412f..f664ebb6 100644 --- a/src/test/container.cpp +++ b/src/test/container.cpp @@ -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 g = std::make_unique(); - 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(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();