From 43bcf99222f9e780bd1bc6fef1ab6c96657040b3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 19 Oct 2017 23:04:57 +0200 Subject: [PATCH] init() throws InitStageException --- include/moveit_task_constructor/container.h | 2 +- include/moveit_task_constructor/stage.h | 32 +++++++++++-- .../stages/current_state.h | 2 +- .../stages/generate_grasp_pose.h | 2 +- .../moveit_task_constructor/stages/gripper.h | 2 +- include/moveit_task_constructor/stages/move.h | 2 +- src/container.cpp | 32 +++++++++---- src/stage.cpp | 46 +++++++++++++++---- src/stage_p.h | 2 +- src/stages/current_state.cpp | 4 +- src/stages/generate_grasp_pose.cpp | 4 +- src/stages/gripper.cpp | 4 +- src/stages/move.cpp | 4 +- src/task.cpp | 5 +- 14 files changed, 103 insertions(+), 40 deletions(-) diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index d93b4568..3d7bf13f 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -37,7 +37,7 @@ public: PRIVATE_CLASS(SerialContainer) SerialContainer(const std::string& name); - bool init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const planning_scene::PlanningSceneConstPtr &scene) override; bool canCompute() const override; bool compute() override; diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index 55d3bab5..448a3a62 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -50,6 +50,32 @@ private: }; +/// 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); + +public: + explicit InitStageException() {} + explicit InitStageException(const Stage& stage, const std::string& msg) { + push_back(stage, msg); + } + + /// push_back a single new error in stage + void push_back(const Stage& stage, const std::string& msg); + /// append all the errors from other (which is emptied) + void append(InitStageException &other); + + /// check of existing errors + operator bool() const { return !errors_.empty(); } + + virtual const char* what() const noexcept override; +private: + std::list> errors_; +}; +std::ostream& operator<<(std::ostream &os, const InitStageException& e); + + class StagePrivate; class Stage { friend std::ostream& operator<<(std::ostream &os, const Stage& stage); @@ -64,7 +90,7 @@ public: operator const StagePrivate*() const; /// initialize stage once before planning - virtual bool init(const planning_scene::PlanningSceneConstPtr& scene); + virtual void init(const planning_scene::PlanningSceneConstPtr& scene); const std::string& name() const; virtual size_t numSolutions() const = 0; @@ -83,7 +109,7 @@ class ComputeBasePrivate; class ComputeBase : public Stage { public: PRIVATE_CLASS(ComputeBase) - bool init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const planning_scene::PlanningSceneConstPtr &scene) override; virtual size_t numSolutions() const override; protected: @@ -105,7 +131,7 @@ public: enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD}; void restrictDirection(Direction dir); - virtual bool init(const planning_scene::PlanningSceneConstPtr &scene) override; + virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override; virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeBackward(const InterfaceState& to) = 0; diff --git a/include/moveit_task_constructor/stages/current_state.h b/include/moveit_task_constructor/stages/current_state.h index 266caf67..18f71ece 100644 --- a/include/moveit_task_constructor/stages/current_state.h +++ b/include/moveit_task_constructor/stages/current_state.h @@ -10,7 +10,7 @@ class CurrentState : public Generator { public: CurrentState(std::string name); - bool init(const planning_scene::PlanningSceneConstPtr& scene) override; + void init(const planning_scene::PlanningSceneConstPtr& scene) override; bool canCompute() const override; bool compute() override; diff --git a/include/moveit_task_constructor/stages/generate_grasp_pose.h b/include/moveit_task_constructor/stages/generate_grasp_pose.h index 41b922d7..2c2b1a5b 100644 --- a/include/moveit_task_constructor/stages/generate_grasp_pose.h +++ b/include/moveit_task_constructor/stages/generate_grasp_pose.h @@ -18,7 +18,7 @@ class GenerateGraspPose : public Generator { public: GenerateGraspPose(std::string name); - bool init(const planning_scene::PlanningSceneConstPtr& scene) override; + void init(const planning_scene::PlanningSceneConstPtr& scene) override; bool canCompute() const override; bool compute() override; diff --git a/include/moveit_task_constructor/stages/gripper.h b/include/moveit_task_constructor/stages/gripper.h index 8dd68321..96eaf318 100644 --- a/include/moveit_task_constructor/stages/gripper.h +++ b/include/moveit_task_constructor/stages/gripper.h @@ -14,7 +14,7 @@ class Gripper : public PropagatingEitherWay { public: Gripper(std::string name); - bool init(const planning_scene::PlanningSceneConstPtr &scene); + void init(const planning_scene::PlanningSceneConstPtr &scene); bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; diff --git a/include/moveit_task_constructor/stages/move.h b/include/moveit_task_constructor/stages/move.h index 3cdbeab0..ccdeda56 100644 --- a/include/moveit_task_constructor/stages/move.h +++ b/include/moveit_task_constructor/stages/move.h @@ -14,7 +14,7 @@ class Move : public Connecting { public: Move(std::string name); - bool init(const planning_scene::PlanningSceneConstPtr &scene); + void init(const planning_scene::PlanningSceneConstPtr &scene); bool compute(const InterfaceState &from, const InterfaceState &to); void setGroup(std::string group); diff --git a/src/container.cpp b/src/container.cpp index 11506674..b6903b4d 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -233,8 +233,10 @@ void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { next->setPrevEnds(prev->ends()); } -bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) +void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) { + InitStageException errors; + auto impl = pimpl(); // clear queues impl->internal_to_my_starts_.clear(); @@ -243,13 +245,20 @@ bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) // recursively init all children for (auto& stage : impl->children()) { - if (!stage->Stage::init(scene) || !stage->init(scene)) - return false; + try { + // should be alled by derived classes too, but you never know... + stage->Stage::init(scene); + stage->init(scene); + } catch (InitStageException &e) { + errors.append(e); + } } // we need to have some children to do the actual work - if (impl->children().empty()) - return false; + if (impl->children().empty()) { + errors.push_back(*this, "no children"); + throw errors; + } // initialize starts_ and ends_ interfaces auto cur = impl->children().begin(); @@ -288,11 +297,16 @@ bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) } // validate connectivity of chain - for (const Stage::pointer& stage : impl->children()) - if (!stage->pimpl()->validate()) - return false; + for (const Stage::pointer& stage : impl->children()) { + try { + stage->pimpl()->validate(); + } catch (InitStageException &e) { + errors.append(e); + } + } - return true; + if (errors) + throw errors; } bool SerialContainer::canCompute() const diff --git a/src/stage.cpp b/src/stage.cpp index 22c4b05f..2ad37a55 100644 --- a/src/stage.cpp +++ b/src/stage.cpp @@ -11,6 +11,30 @@ SubTrajectory::SubTrajectory(StagePrivate *creator, const robot_trajectory::Robo {} +void InitStageException::push_back(const Stage &stage, const std::string &msg) +{ + errors_.emplace_back(std::make_pair(&stage, msg)); +} + +void InitStageException::append(InitStageException &other) +{ + errors_.splice(errors_.end(), other.errors_); +} + +const char *InitStageException::what() const noexcept +{ + static const char* msg = "Error initializing stage(s)"; + return msg; +} + +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; + return os; +} + + StagePrivate::StagePrivate(Stage *me, const std::string &name) : me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr) {} @@ -35,14 +59,17 @@ inline InterfaceFlags StagePrivate::deducedFlags() const } inline bool implies(bool p, bool q) { return !p || q; } -bool StagePrivate::validate() const { +void StagePrivate::validate() const { + InitStageException errors; + InterfaceFlags f = announcedFlags(); if (!implies(f & WRITES_NEXT_START, nextStarts())) - return false; + errors.push_back(*me_, "sends forward, but next stage cannot receive"); if (!implies(f & WRITES_PREV_END, prevEnds())) - return false; - return true; + errors.push_back(*me_, "sends backward, but previous stage cannot receive"); + + if (errors) throw errors; } Stage::Stage(StagePrivate *impl) @@ -64,14 +91,13 @@ Stage::operator const StagePrivate *() const { return pimpl(); } -bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene) +void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) { auto impl = pimpl(); if (impl->starts_) impl->starts_->clear(); if (impl->ends_) impl->ends_->clear(); impl->prev_ends_ = nullptr; impl->next_starts_ = nullptr; - return true; } const std::string& Stage::name() const { @@ -134,9 +160,9 @@ const std::list &ComputeBase::trajectories() const { return pimpl()->trajectories_; } -bool ComputeBase::init(const planning_scene::PlanningSceneConstPtr &scene) { +void ComputeBase::init(const planning_scene::PlanningSceneConstPtr &scene) { pimpl()->trajectories_.clear(); - return Stage::init(scene); + Stage::init(scene); } @@ -274,7 +300,7 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir initInterface(); } -bool PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) +void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) { auto impl = pimpl(); if (!impl->nextStarts()) { @@ -285,7 +311,7 @@ bool PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce impl->ends_.reset(); impl->next_end_state_ = Interface::iterator(); } - return Stage::init(scene); + ComputeBase::init(scene); } void PropagatingEitherWay::sendForward(const InterfaceState& from, diff --git a/src/stage_p.h b/src/stage_p.h index a0d2994f..cb4d91ac 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -53,7 +53,7 @@ public: inline bool isConnected() const { return prev_ends_ || next_starts_; } /// validate that sendForward() and sendBackward() will succeed /// should be only called by containers' init() method - bool validate() const; + void validate() const; inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) { parent_ = parent; diff --git a/src/stages/current_state.cpp b/src/stages/current_state.cpp index dd6bd971..91549d68 100644 --- a/src/stages/current_state.cpp +++ b/src/stages/current_state.cpp @@ -9,11 +9,11 @@ CurrentState::CurrentState(std::string name) ran_= false; } -bool CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene) +void CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene) { + Generator::init(scene); scene_ = scene; ran_= false; - return true; } bool CurrentState::canCompute() const{ diff --git a/src/stages/generate_grasp_pose.cpp b/src/stages/generate_grasp_pose.cpp index 81691c53..aa09a219 100644 --- a/src/stages/generate_grasp_pose.cpp +++ b/src/stages/generate_grasp_pose.cpp @@ -34,10 +34,10 @@ GenerateGraspPose::GenerateGraspPose(std::string name) ros::Duration(1.0).sleep(); } -bool GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) +void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) { + Generator::init(scene); scene_ = scene; - return true; } void GenerateGraspPose::setGroup(std::string group){ diff --git a/src/stages/gripper.cpp b/src/stages/gripper.cpp index 24c4424a..e24e2ab3 100644 --- a/src/stages/gripper.cpp +++ b/src/stages/gripper.cpp @@ -17,10 +17,10 @@ Gripper::Gripper(std::string name) : PropagatingEitherWay(name) {} -bool Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) +void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) { + PropagatingEitherWay::init(scene); planner_ = Task::createPlanner(scene->getRobotModel()); - return true; } void Gripper::setEndEffector(std::string eef){ diff --git a/src/stages/move.cpp b/src/stages/move.cpp index 5a61b8f4..6a5d2db8 100644 --- a/src/stages/move.cpp +++ b/src/stages/move.cpp @@ -18,10 +18,10 @@ Move::Move(std::string name) timeout_(5.0) {} -bool Move::init(const planning_scene::PlanningSceneConstPtr &scene) +void Move::init(const planning_scene::PlanningSceneConstPtr &scene) { + Connecting::init(scene); planner_ = Task::createPlanner(scene->getRobotModel()); - return true; } void Move::setGroup(std::string group){ diff --git a/src/task.cpp b/src/task.cpp index 769958e5..ac5be761 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -119,10 +119,7 @@ bool Task::plan(){ setSolutionCallback(NewSolutionPublisher()); impl->initScene(); - if (!this->init(impl->scene_)) { - ROS_ERROR("init failed"); - return false; - } + init(impl->scene_); while(ros::ok() && canCompute()) { if (compute())