init() throws InitStageException

This commit is contained in:
Robert Haschke 2017-10-19 23:04:57 +02:00
parent 68ff79f464
commit 43bcf99222
14 changed files with 103 additions and 40 deletions

View File

@ -37,7 +37,7 @@ public:
PRIVATE_CLASS(SerialContainer) PRIVATE_CLASS(SerialContainer)
SerialContainer(const std::string& name); 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 canCompute() const override;
bool compute() override; bool compute() override;

View File

@ -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<std::pair<const Stage*, const std::string>> errors_;
};
std::ostream& operator<<(std::ostream &os, const InitStageException& e);
class StagePrivate; class StagePrivate;
class Stage { class Stage {
friend std::ostream& operator<<(std::ostream &os, const Stage& stage); friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
@ -64,7 +90,7 @@ public:
operator const StagePrivate*() const; operator const StagePrivate*() const;
/// initialize stage once before planning /// 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; const std::string& name() const;
virtual size_t numSolutions() const = 0; virtual size_t numSolutions() const = 0;
@ -83,7 +109,7 @@ class ComputeBasePrivate;
class ComputeBase : public Stage { class ComputeBase : public Stage {
public: public:
PRIVATE_CLASS(ComputeBase) 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; virtual size_t numSolutions() const override;
protected: protected:
@ -105,7 +131,7 @@ public:
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 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 computeForward(const InterfaceState& from) = 0;
virtual bool computeBackward(const InterfaceState& to) = 0; virtual bool computeBackward(const InterfaceState& to) = 0;

View File

@ -10,7 +10,7 @@ class CurrentState : public Generator {
public: public:
CurrentState(std::string name); 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 canCompute() const override;
bool compute() override; bool compute() override;

View File

@ -18,7 +18,7 @@ class GenerateGraspPose : public Generator {
public: public:
GenerateGraspPose(std::string name); 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 canCompute() const override;
bool compute() override; bool compute() override;

View File

@ -14,7 +14,7 @@ class Gripper : public PropagatingEitherWay {
public: public:
Gripper(std::string name); 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 computeForward(const InterfaceState& from) override;
bool computeBackward(const InterfaceState& to) override; bool computeBackward(const InterfaceState& to) override;

View File

@ -14,7 +14,7 @@ class Move : public Connecting {
public: public:
Move(std::string name); 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); bool compute(const InterfaceState &from, const InterfaceState &to);
void setGroup(std::string group); void setGroup(std::string group);

View File

@ -233,8 +233,10 @@ void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
next->setPrevEnds(prev->ends()); 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(); auto impl = pimpl();
// clear queues // clear queues
impl->internal_to_my_starts_.clear(); impl->internal_to_my_starts_.clear();
@ -243,13 +245,20 @@ bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
// recursively init all children // recursively init all children
for (auto& stage : impl->children()) { for (auto& stage : impl->children()) {
if (!stage->Stage::init(scene) || !stage->init(scene)) try {
return false; // 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 // we need to have some children to do the actual work
if (impl->children().empty()) if (impl->children().empty()) {
return false; errors.push_back(*this, "no children");
throw errors;
}
// initialize starts_ and ends_ interfaces // initialize starts_ and ends_ interfaces
auto cur = impl->children().begin(); auto cur = impl->children().begin();
@ -288,11 +297,16 @@ bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
} }
// validate connectivity of chain // validate connectivity of chain
for (const Stage::pointer& stage : impl->children()) for (const Stage::pointer& stage : impl->children()) {
if (!stage->pimpl()->validate()) try {
return false; stage->pimpl()->validate();
} catch (InitStageException &e) {
errors.append(e);
}
}
return true; if (errors)
throw errors;
} }
bool SerialContainer::canCompute() const bool SerialContainer::canCompute() const

View File

@ -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) StagePrivate::StagePrivate(Stage *me, const std::string &name)
: me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr) : 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; } inline bool implies(bool p, bool q) { return !p || q; }
bool StagePrivate::validate() const { void StagePrivate::validate() const {
InitStageException errors;
InterfaceFlags f = announcedFlags(); InterfaceFlags f = announcedFlags();
if (!implies(f & WRITES_NEXT_START, nextStarts())) 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())) if (!implies(f & WRITES_PREV_END, prevEnds()))
return false; errors.push_back(*me_, "sends backward, but previous stage cannot receive");
return true;
if (errors) throw errors;
} }
Stage::Stage(StagePrivate *impl) Stage::Stage(StagePrivate *impl)
@ -64,14 +91,13 @@ Stage::operator const StagePrivate *() const {
return pimpl(); return pimpl();
} }
bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene) void Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
auto impl = pimpl(); auto impl = pimpl();
if (impl->starts_) impl->starts_->clear(); if (impl->starts_) impl->starts_->clear();
if (impl->ends_) impl->ends_->clear(); if (impl->ends_) impl->ends_->clear();
impl->prev_ends_ = nullptr; impl->prev_ends_ = nullptr;
impl->next_starts_ = nullptr; impl->next_starts_ = nullptr;
return true;
} }
const std::string& Stage::name() const { const std::string& Stage::name() const {
@ -134,9 +160,9 @@ const std::list<SubTrajectory> &ComputeBase::trajectories() const {
return pimpl()->trajectories_; return pimpl()->trajectories_;
} }
bool ComputeBase::init(const planning_scene::PlanningSceneConstPtr &scene) { void ComputeBase::init(const planning_scene::PlanningSceneConstPtr &scene) {
pimpl()->trajectories_.clear(); pimpl()->trajectories_.clear();
return Stage::init(scene); Stage::init(scene);
} }
@ -274,7 +300,7 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
initInterface(); initInterface();
} }
bool PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
auto impl = pimpl(); auto impl = pimpl();
if (!impl->nextStarts()) { if (!impl->nextStarts()) {
@ -285,7 +311,7 @@ bool PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce
impl->ends_.reset(); impl->ends_.reset();
impl->next_end_state_ = Interface::iterator(); impl->next_end_state_ = Interface::iterator();
} }
return Stage::init(scene); ComputeBase::init(scene);
} }
void PropagatingEitherWay::sendForward(const InterfaceState& from, void PropagatingEitherWay::sendForward(const InterfaceState& from,

View File

@ -53,7 +53,7 @@ public:
inline bool isConnected() const { return prev_ends_ || next_starts_; } inline bool isConnected() const { return prev_ends_ || next_starts_; }
/// validate that sendForward() and sendBackward() will succeed /// validate that sendForward() and sendBackward() will succeed
/// should be only called by containers' init() method /// should be only called by containers' init() method
bool validate() const; void validate() const;
inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) { inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) {
parent_ = parent; parent_ = parent;

View File

@ -9,11 +9,11 @@ CurrentState::CurrentState(std::string name)
ran_= false; ran_= false;
} }
bool CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene) void CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
Generator::init(scene);
scene_ = scene; scene_ = scene;
ran_= false; ran_= false;
return true;
} }
bool CurrentState::canCompute() const{ bool CurrentState::canCompute() const{

View File

@ -34,10 +34,10 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
ros::Duration(1.0).sleep(); 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; scene_ = scene;
return true;
} }
void GenerateGraspPose::setGroup(std::string group){ void GenerateGraspPose::setGroup(std::string group){

View File

@ -17,10 +17,10 @@ Gripper::Gripper(std::string name)
: PropagatingEitherWay(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()); planner_ = Task::createPlanner(scene->getRobotModel());
return true;
} }
void Gripper::setEndEffector(std::string eef){ void Gripper::setEndEffector(std::string eef){

View File

@ -18,10 +18,10 @@ Move::Move(std::string name)
timeout_(5.0) 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()); planner_ = Task::createPlanner(scene->getRobotModel());
return true;
} }
void Move::setGroup(std::string group){ void Move::setGroup(std::string group){

View File

@ -119,10 +119,7 @@ bool Task::plan(){
setSolutionCallback(NewSolutionPublisher()); setSolutionCallback(NewSolutionPublisher());
impl->initScene(); impl->initScene();
if (!this->init(impl->scene_)) { init(impl->scene_);
ROS_ERROR("init failed");
return false;
}
while(ros::ok() && canCompute()) { while(ros::ok() && canCompute()) {
if (compute()) if (compute())