diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index ea00e23b..3d8c4b8b 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -45,10 +45,14 @@ public: enum InterfaceFlag { READS_INPUT = 0x01, READS_OUTPUT = 0x02, - READS_MASK = READS_INPUT | READS_OUTPUT, WRITES_NEXT_INPUT = 0x04, WRITES_PREV_OUTPUT = 0x08, WRITES_UNKNOWN = 0x10, + + OWN_IF_MASK = READS_INPUT | READS_OUTPUT, + EXT_IF_MASK = WRITES_NEXT_INPUT | WRITES_PREV_OUTPUT, + INPUT_IF_MASK = READS_INPUT | WRITES_PREV_OUTPUT | WRITES_UNKNOWN, + OUTPUT_IF_MASK = READS_OUTPUT | WRITES_NEXT_INPUT | WRITES_UNKNOWN, }; typedef Flags InterfaceFlags; InterfaceFlags interfaceFlags() const; diff --git a/src/container.cpp b/src/container.cpp index 434667e4..5a7d56fc 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -65,6 +65,15 @@ bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor } +SubTask::InterfaceFlags SerialContainerPrivate::interfaceFlags() const +{ + SubTask::InterfaceFlags f; + if (!children().size()) return f; + f |= children().front()->pimpl_func()->deducedInterfaceFlags() & SubTask::INPUT_IF_MASK; + f |= children().back()->pimpl_func()->deducedInterfaceFlags() & SubTask::OUTPUT_IF_MASK; + return f; +} + bool SerialContainerPrivate::canInsert(const ContainerBasePrivate::value_type &subtask, ContainerBasePrivate::const_iterator before) const { return ContainerBasePrivate::canInsert(*subtask); } diff --git a/src/container_p.h b/src/container_p.h index bb09b628..7bd4cec6 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -46,6 +46,7 @@ public: output_.reset(new Interface(Interface::NotifyFunction())); } + SubTask::InterfaceFlags interfaceFlags() const override; bool canInsert(const value_type& subtask, const_iterator before) const; const SubTaskPrivate* prev_(const SubTaskPrivate* child) const override; diff --git a/src/subtask.cpp b/src/subtask.cpp index 8e6ee2a1..44e5cdaf 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -13,8 +13,8 @@ SubTask::SubTask(SubTaskPrivate *impl) SubTask::InterfaceFlags SubTask::interfaceFlags() const { SubTask::InterfaceFlags result = pimpl_->interfaceFlags(); - result &= ~SubTask::InterfaceFlags(SubTask::READS_MASK); - result |= pimpl_->SubTaskPrivate::interfaceFlags(); + result &= ~SubTask::InterfaceFlags(SubTask::OWN_IF_MASK); + result |= pimpl_->deducedInterfaceFlags(); return result; } @@ -31,7 +31,20 @@ std::ostream& operator<<(std::ostream &os, const SubTask& stage) { os << *stage.pimpl_func(); return os; } + +template +const char* direction(const SubTaskPrivate& stage) { + SubTask::InterfaceFlags f = stage.deducedInterfaceFlags(); + bool own_if = f & own; + bool other_if = f & other; + bool reverse = own & SubTask::INPUT_IF_MASK; + if (own_if && other_if) return "<>"; + if (!own_if && !other_if) return "--"; + if (other_if ^ reverse) return "->"; + return "<-"; +}; std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) { + // inputs for (const InterfacePtr &i : {stage.prevOutput(), stage.input_}) { os << std::setw(3); @@ -39,7 +52,9 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) { else os << "-"; } // trajectories - os << " -> " << std::setw(3) << stage.trajectories_.size() << " <- "; + os << std::setw(5) << direction(stage) + << std::setw(3) << stage.trajectories_.size() + << std::setw(5) << direction(stage); // outputs for (const InterfacePtr &i : {stage.output_, stage.nextInput()}) { os << std::setw(3); @@ -74,21 +89,23 @@ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajec return trajectories_.back(); } -SubTask::InterfaceFlags SubTaskPrivate::interfaceFlags() const +// return the interface flags that can be deduced from the interface +inline SubTask::InterfaceFlags SubTaskPrivate::deducedInterfaceFlags() const { - // the base class provides the read flags 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; return f; } -void SubTaskPrivate::sendForward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){ +inline void SubTaskPrivate::sendForward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){ std::cout << "sending state to start" << std::endl; nextInput()->add(ps, &trajectory, NULL); } -void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){ +inline void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){ std::cout << "sending state to end" << std::endl; prevOutput()->add(ps, NULL, &trajectory); } diff --git a/src/subtask_p.h b/src/subtask_p.h index 43a7928e..88e464d7 100644 --- a/src/subtask_p.h +++ b/src/subtask_p.h @@ -23,7 +23,8 @@ public: SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost); - virtual SubTask::InterfaceFlags interfaceFlags() const; + SubTask::InterfaceFlags deducedInterfaceFlags() const; + virtual SubTask::InterfaceFlags interfaceFlags() const = 0; void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps); void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);