From c44d0cac19ae96c042e262350419f91c77a80eb5 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 18 Feb 2020 09:31:48 +0100
Subject: [PATCH] workaround for MoveIt PlanningScene bug

https://github.com/ros-planning/moveit/issues/1835
---
 capabilities/src/execute_task_solution_capability.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
index 42ae877b8..dad10a821 100644
--- a/capabilities/src/execute_task_solution_capability.cpp
+++ b/capabilities/src/execute_task_solution_capability.cpp
@@ -173,7 +173,12 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 			if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
 #endif
 				ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
-				return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
+				bool result = context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
+#if MOVEIT_MASTER
+				// HACK: workaround for https://github.com/ros-planning/moveit/issues/1835
+				ros::Duration(0.1).sleep();
+#endif
+				return result;
 			}
 			return true;
 		};