ModifyPlanningScene: check state for collisions

moveit · May 24, 2024 · 9c05305 · 9c05305
1 parent a8896e4
commit 9c05305
Showing 1 changed file with 11 additions and 0 deletions.
diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp
@@ -151,6 +151,17 @@ std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const Interf
 
 		if (callback_)
 			callback_(scene, properties());
+
+		// check for collisions
+		collision_detection::CollisionRequest req;
+		collision_detection::CollisionResult res;
+		req.contacts = true;
+		req.max_contacts = 1;
+		scene->checkCollision(req, res);
+		if (res.collision) {
+			const auto contact = res.contacts.begin()->second.front();
+			traj.markAsFailure(contact.body_name_1 + " colliding with " + contact.body_name_2);
+		}
 	} catch (const std::exception& e) {
 		traj.markAsFailure(e.what());
 	}