diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 80a77ef5..7fbaa996 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -62,6 +62,8 @@ public: /// traverse all children of this container recursively bool traverseRecursively(const StageCallback& processor) const; + void add(Stage::pointer&& stage); + virtual bool insert(Stage::pointer&& stage, int before = -1); virtual bool remove(int pos); virtual bool remove(Stage* child); diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 1e010447..9fe228b0 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -92,8 +92,8 @@ public: /// load robot model from given parameter void loadRobotModel(const std::string& robot_description = "robot_description"); - // TODO: use Stage::insert as well? void add(Stage::pointer&& stage); + bool insert(Stage::pointer&& stage, int before = -1); void clear() final; /// enable introspection publishing for use with rviz diff --git a/core/src/container.cpp b/core/src/container.cpp index 4420c42e..c61a6077 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -194,7 +194,18 @@ bool ContainerBase::traverseRecursively(const ContainerBase::StageCallback& proc return pimpl()->traverseStages(processor, 1, UINT_MAX); } +void ContainerBase::add(Stage::pointer&& stage) { + if (!insert(std::move(stage))) { + throw std::runtime_error(name() + ": Could not insert stage"); + } +} + bool ContainerBase::insert(Stage::pointer&& stage, int before) { + if (!stage) { + ROS_ERROR_STREAM(name() << ": reveived invalid stage pointer"); + return false; + } + StagePrivate* impl = stage->pimpl(); if (!impl->setParent(this)) return false; diff --git a/core/src/task.cpp b/core/src/task.cpp index 5097adfc..cd21b39d 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -194,11 +194,11 @@ void Task::loadRobotModel(const std::string& robot_description) { } void Task::add(Stage::pointer&& stage) { - if (!stage) - throw std::runtime_error("stage insertion failed: invalid stage pointer"); + stages()->add(std::move(stage)); +} - if (!stages()->insert(std::move(stage))) - throw std::runtime_error(std::string("insertion failed for stage: ") + stage->name()); +bool Task::insert(Stage::pointer&& stage, int before) { + return stages()->insert(std::move(stage), before); } void Task::clear() {