fixed some small problems

src/mediator/mg_mediator.cpp

@@ -2,13 +2,13 @@
 
 #include "yaml-cpp/yaml.h"
 
-std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> shared_structure;
+std::vector<std::pair<std::string,moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> shared_structure;
 std::mutex shared_mutex;
 std::condition_variable cv;
 
 void MGMediator::processSharedStructure() {
   while (ros::ok()) {
-	std::vector<moveit_task_constructor_msgs::SubTrajectory> peak;
+	std::vector<std::pair<std::string, moveit_task_constructor_msgs::SubTrajectory>> peak;
 
 	{
 		std::unique_lock<std::mutex> lock(shared_mutex);
@@ -18,21 +18,41 @@ void MGMediator::processSharedStructure() {
 
 		// Iterate over the vectors in shared_structure
 		for (auto it = shared_structure.begin(); it != shared_structure.end(); ++it) {
-			if (!it->solution.sub_trajectory.empty()) {
+			if (!it->second.solution.sub_trajectory.empty()) {
 			// Get the first entry of the vector and add it to peak
-			peak.push_back(it->solution.sub_trajectory.front());
-			it->solution.sub_trajectory.erase(it->solution.sub_trajectory.begin());
+			peak.push_back(std::make_pair(it->first, it->second.solution.sub_trajectory.front()));
+			it->second.solution.sub_trajectory.erase(it->second.solution.sub_trajectory.begin());
 			}
 
-			if (it->solution.sub_trajectory.empty()) {
-			// Remove the vector if all entries have been processed
+			if (it->second.solution.sub_trajectory.empty() && it->first == "panda_arm1") {
+				/*
+				All sub_trajectorys of an Robot are done, so the grammar should be informed about the state
+				*/
+				std::stringstream ss;
+				ss << it->first << "/idle";
+				sendToAll(ss.str(), "");
+
 				it = shared_structure.erase(it);
 				--it; // Adjust the iterator after erasing
 			}
       	}
+
+	} 
+			
+	for (const auto& exec : peak){
+		if(exec.first == "panda_arm1"){
+			AbstractRobotDecorator* mr = nullptr;
+    		try{
+        		mr= robots_.at(exec.first).get();
+				mr->mgi()->execute(exec.second.trajectory);
+				planning_scene_diff_publisher_->publish(exec.second.scene_diff);
+    		} catch (std::out_of_range& oor){
+        		ROS_INFO("Robot not found");
+        		ros::shutdown();
+    		} 
+		}
 	}
 
-	sendToAll("payload/panda_arm1", "penis");
 	// The lock is automatically released here when `lock` goes out of scope
 
 	// Print the sizes of the sub_trajectory vectors
@@ -138,13 +158,13 @@ void MGMediator::setupMqtt(){
 				moveit::task_constructor::Task mgt = pickUp(node["object"].as<std::string>(), node["robot"].as<std::string>());
 				if(mgt.plan(1)) {
 					//mgt.introspection().publishSolution(*mgt.solutions().front());
-					mgt.execute(*mgt.solutions().front());
+					//mgt.execute(*mgt.solutions().front());
 					moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
 					mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
 
 					{
 						std::lock_guard<std::mutex> lock(shared_mutex);
-						shared_structure.push_back(std::move(e));
+						shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(), std::move(e)));
 					}
 
 					// Notify the consuming thread
@@ -156,13 +176,12 @@ void MGMediator::setupMqtt(){
 				moveit::task_constructor::Task mgt = place(node["object"].as<std::string>(), node["robot"].as<std::string>(), node["place"].as<std::string>());
 				if(mgt.plan(1)) {
 					//mgt.introspection().publishSolution(*mgt.solutions().front());
-					mgt.execute(*mgt.solutions().front());
 					moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
 					mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
 
 					{
 						std::lock_guard<std::mutex> lock(shared_mutex);
-						shared_structure.push_back(std::move(e));
+						shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(),std::move(e)));
 					}
 
 					// Notify the consuming thread
@@ -174,13 +193,12 @@ void MGMediator::setupMqtt(){
 				moveit::task_constructor::Task mgt = drop(node["object"].as<std::string>(), node["robot"].as<std::string>(), node["drop"].as<std::string>());
 				if(mgt.plan(1)) {
 					//mgt.introspection().publishSolution(*mgt.solutions().front());
-					mgt.execute(*mgt.solutions().front());
 					moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
 					mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
 
 					{
 						std::lock_guard<std::mutex> lock(shared_mutex);
-						shared_structure.push_back(std::move(e));
+						shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(),std::move(e)));
 					}
 
 					// Notify the consuming thread