mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
init() throws InitStageException
This commit is contained in:
parent
68ff79f464
commit
43bcf99222
@ -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;
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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,
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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{
|
||||||
|
|||||||
@ -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){
|
||||||
|
|||||||
@ -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){
|
||||||
|
|||||||
@ -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){
|
||||||
|
|||||||
@ -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())
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user