diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp index faa005405999e2fe070360c8d091ab47e89ea440..216988e758df9ee72f39bb5db47970f62c78aa0d 100644 --- a/src/mediator/mg_mediator.cpp +++ b/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 - it = shared_structure.erase(it); - --it; // Adjust the iterator after erasing + 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 } } + } - - sendToAll("payload/panda_arm1", "penis"); + + 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(); + } + } + } + // 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 @@ -401,7 +419,7 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s } catch (std::out_of_range& oor){ ROS_INFO("Robot not found"); ros::shutdown(); - } + } const std::string object = obj; moveit::task_constructor::Task task_;