mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Add ability to add/remove objects to/from planning scene (#165)
This commit is contained in:
parent
2d99017c17
commit
b3b215a4f7
@ -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);
|
||||||
|
|||||||
@ -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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user