Add ability to add/remove objects to/from planning scene (#165)

Author: JafarAbdi

core/include/moveit/task_constructor/stages/modify_planning_scene.h

@@ -41,6 +41,7 @@
 #include <moveit/task_constructor/stage.h>
 #include <moveit/task_constructor/properties.h>
 #include <moveit/task_constructor/type_traits.h>
+#include <moveit_msgs/CollisionObject.h>
 #include <map>
 
 namespace moveit {
@@ -77,6 +78,10 @@ class ModifyPlanningScene : public PropagatingEitherWay
 
 	/// attach or detach a list of objects to the given link
 	void attachObjects(const Names& objects, const std::string& attach_link, bool attach);
+	/// Add an object to the planning scene
+	void addObject(const moveit_msgs::CollisionObject& collision_object);
+	/// Remove an object from the planning scene
+	void removeObject(const std::string& object_name);
 
 	/// conviency methods accepting a single object name
 	inline void attachObject(const std::string& object, const std::string& link);
@@ -130,6 +135,8 @@ class ModifyPlanningScene : public PropagatingEitherWay
 protected:
 	// list of objects to attach (true) / detach (false) to a given link
 	std::map<std::string, std::pair<Names, bool>> attach_objects_;
+	// list of objects to add / remove to the planning scene
+	std::vector<moveit_msgs::CollisionObject> collision_objects_;
 
 	// list of objects to mutually
 	struct CollisionMatrixPairs
@@ -144,6 +151,7 @@ class ModifyPlanningScene : public PropagatingEitherWay
 protected:
 	// apply stored modifications to scene
 	InterfaceState apply(const InterfaceState& from, bool invert);
+	void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object);
 	void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
 	                   bool invert);
 	void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);

core/src/stages/modify_planning_scene.cpp

@@ -52,6 +52,22 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string&
 	o.insert(o.end(), objects.begin(), objects.end());
 }
 
+void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collision_object) {
+	if (collision_object.operation != moveit_msgs::CollisionObject::ADD) {
+		ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": addObject is called with object's operation not set "
+		                                                        "to ADD -- ignoring the object");
+		return;
+	}
+	collision_objects_.push_back(collision_object);
+}
+
+void ModifyPlanningScene::removeObject(const std::string& object_name) {
+	moveit_msgs::CollisionObject obj;
+	obj.id = object_name;
+	obj.operation = moveit_msgs::CollisionObject::REMOVE;
+	collision_objects_.push_back(obj);
+}
+
 void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) {
 	collision_matrix_edits_.push_back(CollisionMatrixPairs({ first, second, allow }));
 }
@@ -102,6 +118,9 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene,
 InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
 	planning_scene::PlanningScenePtr scene = from.scene()->diff();
 	InterfaceState result(scene);
+	// add/remove objects
+	for (const auto& collision_object : collision_objects_)
+		processCollisionObject(*scene, collision_object);
 
 	// attach/detach objects
 	for (const auto& pair : attach_objects_)
@@ -116,6 +135,11 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
 
 	return result;
 }
+
+void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
+                                                 const moveit_msgs::CollisionObject& object) {
+	scene.processCollisionObjectMsg(object);
+}
 }
 }
 }