mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Remove return value from Container::insert
adapted Container::add Logical consequence of removing the ROS_ERROR output in `setParent`
This commit is contained in:
parent
4386d0cc60
commit
a6fa45214f
@ -64,7 +64,7 @@ public:
|
||||
|
||||
void add(Stage::pointer&& stage);
|
||||
|
||||
virtual bool insert(Stage::pointer&& stage, int before = -1);
|
||||
virtual void insert(Stage::pointer&& stage, int before = -1);
|
||||
virtual Stage::pointer remove(int pos);
|
||||
virtual Stage::pointer remove(Stage* child);
|
||||
virtual void clear();
|
||||
@ -205,7 +205,7 @@ public:
|
||||
WrapperBase(const std::string& name = "wrapper", Stage::pointer&& child = Stage::pointer());
|
||||
|
||||
/// insertion is only allowed if children() is empty
|
||||
bool insert(Stage::pointer&& stage, int before = -1) override;
|
||||
void insert(Stage::pointer&& stage, int before = -1) override;
|
||||
|
||||
/// access the single wrapped child, NULL if still empty
|
||||
Stage* wrapped();
|
||||
|
||||
@ -112,13 +112,12 @@ public:
|
||||
|
||||
/// set parent of stage
|
||||
/// enforce only one parent exists
|
||||
inline bool setParent(ContainerBase* parent) {
|
||||
inline void setParent(ContainerBase* parent) {
|
||||
if (parent_) {
|
||||
// it's not allowed to add a stage to a parent if it already has one
|
||||
throw std::runtime_error("Tried to add stage '" + name() + "' to two parents");
|
||||
}
|
||||
parent_ = parent;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// explicitly orphan stage
|
||||
|
||||
@ -94,7 +94,7 @@ public:
|
||||
void loadRobotModel(const std::string& robot_description = "robot_description");
|
||||
|
||||
void add(Stage::pointer&& stage);
|
||||
bool insert(Stage::pointer&& stage, int before = -1) override;
|
||||
void insert(Stage::pointer&& stage, int before = -1) override;
|
||||
void clear() final;
|
||||
|
||||
/// enable introspection publishing for use with rviz
|
||||
|
||||
@ -182,24 +182,18 @@ bool ContainerBase::traverseRecursively(const ContainerBase::StageCallback& proc
|
||||
}
|
||||
|
||||
void ContainerBase::add(Stage::pointer&& stage) {
|
||||
if (!insert(std::move(stage))) {
|
||||
throw std::runtime_error(name() + ": Could not insert stage");
|
||||
}
|
||||
insert(std::move(stage), -1);
|
||||
}
|
||||
|
||||
bool ContainerBase::insert(Stage::pointer&& stage, int before) {
|
||||
if (!stage) {
|
||||
ROS_ERROR_STREAM(name() << ": received invalid stage pointer");
|
||||
return false;
|
||||
}
|
||||
void ContainerBase::insert(Stage::pointer&& stage, int before) {
|
||||
if (!stage)
|
||||
throw std::runtime_error(name() + ": received invalid stage pointer");
|
||||
|
||||
StagePrivate* impl = stage->pimpl();
|
||||
if (!impl->setParent(this))
|
||||
return false;
|
||||
impl->setParent(this);
|
||||
ContainerBasePrivate::const_iterator where = pimpl()->childByIndex(before, true);
|
||||
ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage));
|
||||
impl->setParentPosition(it);
|
||||
return true;
|
||||
}
|
||||
|
||||
Stage::pointer ContainerBasePrivate::remove(ContainerBasePrivate::const_iterator pos) {
|
||||
@ -636,10 +630,10 @@ WrapperBase::WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child) : Par
|
||||
WrapperBase::insert(std::move(child));
|
||||
}
|
||||
|
||||
bool WrapperBase::insert(Stage::pointer&& stage, int before) {
|
||||
void WrapperBase::insert(Stage::pointer&& stage, int before) {
|
||||
// restrict num of children to one
|
||||
if (numChildren() > 0)
|
||||
return false;
|
||||
throw std::runtime_error(name() + ": Wrapper only allows a single child");
|
||||
return ParallelContainerBase::insert(std::move(stage), before);
|
||||
}
|
||||
|
||||
|
||||
@ -203,8 +203,8 @@ void Task::add(Stage::pointer&& stage) {
|
||||
stages()->add(std::move(stage));
|
||||
}
|
||||
|
||||
bool Task::insert(Stage::pointer&& stage, int before) {
|
||||
return stages()->insert(std::move(stage), before);
|
||||
void Task::insert(Stage::pointer&& stage, int before) {
|
||||
stages()->insert(std::move(stage), before);
|
||||
}
|
||||
|
||||
void Task::clear() {
|
||||
|
||||
@ -298,28 +298,28 @@ TEST_F(SerialTest, insertion_order) {
|
||||
/***** inserting first stage *****/
|
||||
auto g = std::make_unique<GeneratorMockup>();
|
||||
StagePrivate* gp = g->pimpl();
|
||||
ASSERT_TRUE(container.insert(std::move(g)));
|
||||
container.insert(std::move(g));
|
||||
EXPECT_FALSE(g); // ownership transferred to container
|
||||
VALIDATE(gp);
|
||||
|
||||
/***** inserting second stage *****/
|
||||
auto f = std::make_unique<ForwardMockup>();
|
||||
StagePrivate* fp = f->pimpl();
|
||||
ASSERT_TRUE(container.insert(std::move(f)));
|
||||
container.insert(std::move(f));
|
||||
EXPECT_FALSE(f); // ownership transferred to container
|
||||
VALIDATE(gp, fp);
|
||||
|
||||
/***** inserting third stage *****/
|
||||
auto f2 = std::make_unique<ForwardMockup>();
|
||||
StagePrivate* fp2 = f2->pimpl();
|
||||
ASSERT_TRUE(container.insert(std::move(f2), 1));
|
||||
container.insert(std::move(f2), 1);
|
||||
EXPECT_FALSE(f2); // ownership transferred to container
|
||||
VALIDATE(gp, fp2, fp);
|
||||
|
||||
/***** inserting another generator stage *****/
|
||||
auto g2 = std::make_unique<GeneratorMockup>();
|
||||
StagePrivate* gp2 = g2->pimpl();
|
||||
ASSERT_TRUE(container.insert(std::move(g2)));
|
||||
container.insert(std::move(g2));
|
||||
VALIDATE(gp, fp2, fp, gp2);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user