diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index f884e026..6c42aeac 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -76,6 +76,7 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); }; +std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); class SerialContainerPrivate; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 0f093ef4..b9f51300 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -86,7 +86,7 @@ typedef std::pair InterfaceStatePa /// exception thrown by Stage::init() /// It collects individual errors in stages throughout the pipeline to allow overall error reporting class InitStageException : public std::exception { - friend std::ostream& operator<<(std::ostream &os, const InitStageException& e); + friend std::ostream& operator<<(std::ostream& os, const InitStageException& e); public: explicit InitStageException() {} @@ -112,8 +112,6 @@ std::ostream& operator<<(std::ostream &os, const InitStageException& e); class ContainerBase; class StagePrivate; class Stage { - friend std::ostream& operator<<(std::ostream &os, const Stage& stage); - public: PRIVATE_CLASS(Stage) typedef std::unique_ptr pointer; @@ -175,7 +173,7 @@ protected: protected: StagePrivate* const pimpl_; // constness guarantees one initial write }; -std::ostream& operator<<(std::ostream &os, const Stage& stage); +std::ostream& operator<<(std::ostream& os, const Stage& stage); class ComputeBasePrivate; @@ -299,5 +297,4 @@ public: } }; - } } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index a9d2ab80..2f8b1de1 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -52,7 +52,7 @@ namespace moveit { namespace task_constructor { class ContainerBase; class StagePrivate { friend class Stage; - friend std::ostream& operator<<(std::ostream &os, const StagePrivate& stage); + friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); public: typedef std::list container_type; @@ -120,7 +120,7 @@ private: Introspection* introspection_; // task's introspection instance }; PIMPL_FUNCTIONS(Stage) -std::ostream& operator<<(std::ostream &os, const StagePrivate& stage); +std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); // ComputeBasePrivate is the base class for all computing stages, i.e. non-containers. diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 7d1fbe5d..4620ca70 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -139,4 +139,9 @@ private: std::list task_cbs_; // functions to monitor task's planning progress }; +inline std::ostream& operator<<(std::ostream& os, const Task& task) { + task.printState(os); + return os; +} + } } diff --git a/core/src/container.cpp b/core/src/container.cpp index a8b9f4a6..bddb5187 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -225,6 +225,15 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) throw errors; } +std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { + ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool { + os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl; + return true; + }; + container.traverseRecursively(processor); + return os; +} + struct SolutionCollector { SolutionCollector(size_t max_depth) : max_depth(max_depth) {} diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ca8c338b..3f86dfe6 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -61,7 +61,7 @@ const char *InitStageException::what() const noexcept return msg; } -std::ostream& operator<<(std::ostream &os, const InitStageException& e) { +std::ostream& operator<<(std::ostream& os, const InitStageException& e) { os << e.what() << std::endl; for (const auto &pair : e.errors_) os << pair.first->name() << ": " << pair.second << std::endl; @@ -183,26 +183,25 @@ const char* direction(const StagePrivate& stage) { return "<-"; } -std::ostream& operator<<(std::ostream &os, const Stage& stage) { - auto impl = stage.pimpl(); +std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { // starts - for (const InterfaceConstPtr& i : {impl->prevEnds(), impl->starts()}) { + for (const InterfaceConstPtr& i : {impl.prevEnds(), impl.starts()}) { os << std::setw(3); if (i) os << i->size(); else os << "-"; } // trajectories - os << std::setw(5) << direction(*impl) - << std::setw(3) << stage.numSolutions() - << std::setw(5) << direction(*impl); + os << std::setw(5) << direction(impl) + << std::setw(3) << impl.me()->numSolutions() + << std::setw(5) << direction(impl); // ends - for (const InterfaceConstPtr& i : {impl->ends(), impl->nextStarts()}) { + for (const InterfaceConstPtr& i : {impl.ends(), impl.nextStarts()}) { os << std::setw(3); if (i) os << i->size(); else os << "-"; } // name - os << " / " << stage.name(); + os << " / " << impl.name(); return os; } @@ -550,4 +549,9 @@ void Connecting::connect(const InterfaceState& from, const InterfaceState& to, impl->newSolution(trajectory); } +std::ostream& operator<<(std::ostream& os, const Stage& stage) { + os << *stage.pimpl(); + return os; +} + } } diff --git a/core/src/task.cpp b/core/src/task.cpp index 6d2a1c06..f2065b24 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -286,13 +286,8 @@ std::string Task::id() const return id_; } -void Task::printState(std::ostream& os) const -{ - ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool { - os << std::string(2*depth, ' ') << stage << std::endl; - return true; - }; - stages()->traverseRecursively(processor); +void Task::printState(std::ostream& os) const { + os << *stages(); } } }