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

This commit is contained in:
Jafar Abdi 2020-05-05 19:28:54 +03:00 committed by GitHub
parent 2d99017c17
commit b3b215a4f7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 32 additions and 0 deletions

View File

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

View File

@ -52,6 +52,22 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string&
o.insert(o.end(), objects.begin(), objects.end()); 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) { void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) {
collision_matrix_edits_.push_back(CollisionMatrixPairs({ first, second, 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) { InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
planning_scene::PlanningScenePtr scene = from.scene()->diff(); planning_scene::PlanningScenePtr scene = from.scene()->diff();
InterfaceState result(scene); InterfaceState result(scene);
// add/remove objects
for (const auto& collision_object : collision_objects_)
processCollisionObject(*scene, collision_object);
// attach/detach objects // attach/detach objects
for (const auto& pair : attach_objects_) for (const auto& pair : attach_objects_)
@ -116,6 +135,11 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
return result; return result;
} }
void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
const moveit_msgs::CollisionObject& object) {
scene.processCollisionObjectMsg(object);
}
} }
} }
} }