From 9c05305effa7173a94f3befdae09386763e04f0a Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 24 May 2024 10:49:14 +0200
Subject: [PATCH] ModifyPlanningScene: check state for collisions

---
 core/src/stages/modify_planning_scene.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp
index c321e9d1e..c24a18a1d 100644
--- 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());
 	}