mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Add ability to move CollisionObjects (#567)
This commit is contained in:
parent
819e560ed4
commit
ad19ea5479
@ -81,6 +81,8 @@ public:
|
||||
void addObject(const moveit_msgs::CollisionObject& collision_object);
|
||||
/// Remove an object from the planning scene
|
||||
void removeObject(const std::string& object_name);
|
||||
/// Move an object from the planning scene
|
||||
void moveObject(const moveit_msgs::CollisionObject& collision_object);
|
||||
|
||||
/// conviency methods accepting a single object name
|
||||
inline void attachObject(const std::string& object, const std::string& link);
|
||||
|
||||
@ -72,6 +72,15 @@ void ModifyPlanningScene::removeObject(const std::string& object_name) {
|
||||
collision_objects_.push_back(obj);
|
||||
}
|
||||
|
||||
void ModifyPlanningScene::moveObject(const moveit_msgs::CollisionObject& collision_object) {
|
||||
if (collision_object.operation != moveit_msgs::CollisionObject::MOVE) {
|
||||
ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": moveObject is called with object's operation not set "
|
||||
"to MOVE -- ignoring the object");
|
||||
return;
|
||||
}
|
||||
collision_objects_.push_back(collision_object);
|
||||
}
|
||||
|
||||
void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) {
|
||||
collision_matrix_edits_.push_back(CollisionMatrixPairs({ first, second, allow }));
|
||||
}
|
||||
@ -128,7 +137,7 @@ std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const Interf
|
||||
InterfaceState state(scene);
|
||||
SubTrajectory traj;
|
||||
try {
|
||||
// add/remove objects
|
||||
// add/remove/move objects
|
||||
for (auto& collision_object : collision_objects_)
|
||||
processCollisionObject(*scene, collision_object, invert);
|
||||
|
||||
@ -157,6 +166,8 @@ void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene&
|
||||
const_cast<moveit_msgs::CollisionObject&>(object).operation = moveit_msgs::CollisionObject::REMOVE;
|
||||
else if (op == moveit_msgs::CollisionObject::REMOVE)
|
||||
throw std::runtime_error("cannot apply removeObject() backwards");
|
||||
else if (op == moveit_msgs::CollisionObject::MOVE)
|
||||
throw std::runtime_error("cannot apply moveObject() backwards");
|
||||
}
|
||||
|
||||
scene.processCollisionObjectMsg(object);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user