diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 85bacfef..6248815e 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace moveit { @@ -77,6 +78,10 @@ public: /// 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 @@ public: protected: // list of objects to attach (true) / detach (false) to a given link std::map> attach_objects_; + // list of objects to add / remove to the planning scene + std::vector collision_objects_; // list of objects to mutually struct CollisionMatrixPairs @@ -144,6 +151,7 @@ protected: 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>& pair, bool invert); void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert); diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 7ee462a4..4b18d15a 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/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); +} } } }