streamline add/insert interfaces for Task/Container

`add` falls back to `insert` for both structures,
but `add` throws exceptions and does not provide a return value.

`insert` provides standard STL container access.

Author: v4hn

core/include/moveit/task_constructor/container.h

@@ -62,6 +62,8 @@ class ContainerBase : public Stage
 	/// 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);

core/include/moveit/task_constructor/task.h

@@ -92,8 +92,8 @@ class Task : protected WrapperBase
 	/// 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

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;

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() {