Skip to content
Snippets Groups Projects
Commit f64b0bbb authored by KingMaZito's avatar KingMaZito
Browse files

some mutex changed

parent bc0471c5
Branches
No related tags found
No related merge requests found
...@@ -32,6 +32,31 @@ void MGMediator::processSharedStructure() { ...@@ -32,6 +32,31 @@ void MGMediator::processSharedStructure() {
ss << it->first << "/idle"; ss << it->first << "/idle";
sendToAll(ss.str(), ""); 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 = shared_structure.erase(it);
--it; // Adjust the iterator after erasing --it; // Adjust the iterator after erasing
} }
...@@ -155,6 +180,9 @@ void MGMediator::setupMqtt(){ ...@@ -155,6 +180,9 @@ void MGMediator::setupMqtt(){
} catch (const YAML::RepresentationException& e){ ROS_INFO("Bad Command"); return;} } catch (const YAML::RepresentationException& e){ ROS_INFO("Bad Command"); return;}
if (node["mode"].as<std::string>() == "PICKUP"){ 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>()); moveit::task_constructor::Task mgt = pickUp(node["object"].as<std::string>(), node["robot"].as<std::string>());
if(mgt.plan(1)) { if(mgt.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front()); //mgt.introspection().publishSolution(*mgt.solutions().front());
...@@ -162,49 +190,49 @@ void MGMediator::setupMqtt(){ ...@@ -162,49 +190,49 @@ void MGMediator::setupMqtt(){
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); 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))); shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(), std::move(e)));
}
// Notify the consuming thread // Notify the consuming thread
cv.notify_one(); cv.notify_one();
} }
} }
}
if (node["mode"].as<std::string>() == "PLACE"){ 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>()); 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)) { if(mgt.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front()); //mgt.introspection().publishSolution(*mgt.solutions().front());
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); 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))); shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(),std::move(e)));
}
// Notify the consuming thread // Notify the consuming thread
cv.notify_one(); cv.notify_one();
} }
} }
}
if (node["mode"].as<std::string>() == "DROP"){ 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>()); 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)) { if(mgt.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front()); //mgt.introspection().publishSolution(*mgt.solutions().front());
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); 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))); shared_structure.push_back(std::make_pair(node["robot"].as<std::string>(),std::move(e)));
}
// Notify the consuming thread // Notify the consuming thread
cv.notify_one(); cv.notify_one();
} }
} }
}
}); });
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment