diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp index 6c277b462afab3a90a48cabb89596dd16b92bc82..a298446ddda552cfe08d00328facb913c8c54112 100644 --- a/src/mediator/mg_mediator.cpp +++ b/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,54 +180,57 @@ void MGMediator::setupMqtt(){ } catch (const YAML::RepresentationException& e){ ROS_INFO("Bad Command"); return;} if (node["mode"].as<std::string>() == "PICKUP"){ - 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()); - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; - mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); - - { - std::lock_guard<std::mutex> lock(shared_mutex); + + { + 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()); + //mgt.execute(*mgt.solutions().front()); + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; + mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); + shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(), std::move(e))); - } - // Notify the consuming thread - cv.notify_one(); + // Notify the consuming thread + cv.notify_one(); + } } } if (node["mode"].as<std::string>() == "PLACE"){ - 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); + + { + 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()); + shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(),std::move(e))); - } - // Notify the consuming thread - cv.notify_one(); + // Notify the consuming thread + cv.notify_one(); + } } } if (node["mode"].as<std::string>() == "DROP"){ - 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); + + { + 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()); + shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(),std::move(e))); + + // Notify the consuming thread + cv.notify_one(); } - - // Notify the consuming thread - cv.notify_one(); } } });