some mutex changed

src/mediator/mg_mediator.cpp

@@ -32,6 +32,31 @@ void MGMediator::processSharedStructure() {
 				ss << it->first << "/idle";
 				sendToAll(ss.str(), "");
 
+				/*
+					Lets publish also scene modifications here
+					But I want to publish only vanished objects
+					TODO: Implement your own stage, so you don't have to do this embarrassing loops
+				*/
+
+				std::string temp;
+				if ((cuboid_reader_->cuboidBox().size() + cuboid_reader_->cuboidBox().size()) > psi_->getObjects().size()){
+					ROS_INFO("object is vanished!");
+					try{
+						for (const auto& c : cuboid_reader_->cuboidBox()){
+							temp = c.Name;
+							psi_->getObjects().at(c.Name);
+						}
+					} catch (const std::out_of_range& e) {
+						std::stringstream payload;
+						payload << "{\"type\": \"MODIFY\", \"mode\": \"DELETE\", \"robot\": \""<< it->first <<"\", \"object\": \""<< temp <<"\" }";
+
+						std::stringstream payload_topic;
+						payload_topic << it->first << "/payload";
+						sendToAll(payload_topic.str(), payload.str());
+            		}	
+				}
+
+
 				it = shared_structure.erase(it);
 				--it; // Adjust the iterator after erasing
 			}
@@ -155,6 +180,9 @@ void MGMediator::setupMqtt(){
 			} catch (const YAML::RepresentationException& e){ ROS_INFO("Bad Command"); return;}
 			
 			if (node["mode"].as<std::string>() == "PICKUP"){
+
+				{
+					std::unique_lock<std::mutex> lock(shared_mutex);
 					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());
@@ -162,49 +190,49 @@ void MGMediator::setupMqtt(){
 						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::make_pair(node["robot"].as<std::string>(), std::move(e)));
-					}
 
 						// Notify the consuming thread
 						cv.notify_one();
 					}
 				}
+			}
 
 			if (node["mode"].as<std::string>() == "PLACE"){
+
+				{
+					std::unique_lock<std::mutex> lock(shared_mutex);
 					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());
 						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::make_pair(node["robot"].as<std::string>(),std::move(e)));
-					}
 
 						// Notify the consuming thread
 						cv.notify_one();
 					}
 				}
+			}
 
 			if (node["mode"].as<std::string>() == "DROP"){
+
+				{
+					std::unique_lock<std::mutex> lock(shared_mutex);
 					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());
 						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::make_pair(node["robot"].as<std::string>(),std::move(e)));
-					}
 						
 						// Notify the consuming thread
 						cv.notify_one();
 					}
 				}
+			}
 		});
 	}