diff --git a/.vscode/settings.json b/.vscode/settings.json
index 8cd90b9b69d6c912c29e3759fd26beb95341d691..97fdc20dae96c148cb1100a7db3b29a78fb2da0c 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -75,7 +75,8 @@
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp",
- "future": "cpp"
+ "future": "cpp",
+ "*.ipp": "cpp"
},
"python.analysis.extraPaths": [
"/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages",
diff --git a/include/mediator/mg_mediator.h b/include/mediator/mg_mediator.h
index 3c661f171565f5d843bbe1e55806372ed65c6c29..4f1845b1495451d3fe85ca708c64de29d0b7896c 100644
--- a/include/mediator/mg_mediator.h
+++ b/include/mediator/mg_mediator.h
@@ -77,6 +77,8 @@ class MGMediator: public AbstractMediator, virtual Controller{
void publishTables();
moveit::task_constructor::Task pickUp(const std::string& obj, const std::string& robotId);
moveit::task_constructor::Task place(const std::string& obj, const std::string& robotId, const std::string& target);
+ void processSharedStructure();
+
};
#endif
\ No newline at end of file
diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp
index 7bd9d808c9880950210c95186e2d6891d8a5b920..bfe9c159b5c3abac8acd202f0533c63138db0058 100644
--- a/src/mediator/mg_mediator.cpp
+++ b/src/mediator/mg_mediator.cpp
@@ -1,5 +1,51 @@
#include "mediator/mg_mediator.h"
+std::vector<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::unique_lock<std::mutex> lock(shared_mutex);
+
+ // Wait until the shared structure is not empty
+ cv.wait(lock, []{ return !shared_structure.empty(); });
+
+ // Iterate over the vectors in shared_structure
+ for (auto it = shared_structure.begin(); it != shared_structure.end(); ++it) {
+ if (!it->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());
+ }
+
+ 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
+ }
+ }
+ } // The lock is automatically released here when `lock` goes out of scope
+
+ // Print the sizes of the sub_trajectory vectors
+ // if (!shared_structure.empty()) {
+ // for (const auto& temp : shared_structure) {
+ // ROS_INFO("%zu", temp.solution.sub_trajectory.size());
+ // }
+ // }
+
+ // ROS_INFO("Do something");
+
+ peak.clear();
+
+ // Sleep or do other work
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+}
+
MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
: AbstractMediator(nh)
, Controller(*nh.get())
@@ -10,14 +56,30 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1))){
// MQTT HANDLE
+ auto mqtt = std::make_shared<MqttConnection>("localhost", "op");
+ mqtt->listen("command/place");
+ mqtt->listen("command/pickup");
+
+ addConnection(std::move(mqtt));
+
addCallback("command/pickup", [&](const std::string& data) {
std::vector<std::pair<std::string, std::string>> pickup_command{std::make_pair("A", "panda_arm1"), std::make_pair("B", "panda_arm2") };
for (const auto& pair: pickup_command) {
moveit::task_constructor::Task mgt = pickUp(pair.first, pair.second);
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));
+ }
+
+ // Notify the consuming thread
+ cv.notify_one();
+ }
}
});
@@ -27,17 +89,28 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
moveit::task_constructor::Task mgt = place(std::get<0>(tuple), std::get<1>(tuple), std::get<2>(tuple));
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));
+ }
+
+ // Notify the consuming thread
+ cv.notify_one();
+
}
}
});
- moveit::task_constructor::Task mgt2 = place("A", "panda_arm1", "dafd");
- if(mgt2.plan(1)) {
- //mgt.introspection().publishSolution(*mgt.solutions().front());
- mgt2.execute(*mgt2.solutions().front());
- }
+ // moveit::task_constructor::Task mgt2 = place("A", "panda_arm1", "dafd");
+ // if(mgt2.plan(1)) {
+ // //mgt.introspection().publishSolution(*mgt.solutions().front());
+ // mgt2.execute(*mgt2.solutions().front());
+ // }
robot_model_loader::RobotModelLoaderPtr robot_model_loader;
@@ -62,18 +135,23 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
- addConnection(std::make_shared<MqttConnection>("localhost:1883", "asdfadf"));
}
void MGMediator::mediate(){
setPanel();
publishTables();
- moveit::task_constructor::Task mgt1 = pickUp("A", "panda_arm1");
- if(mgt1.plan(1)) {
- //mgt.introspection().publishSolution(*mgt.solutions().front());
- mgt1.execute(*mgt1.solutions().front());
- }
+ // moveit::task_constructor::Task mgt1 = pickUp("A", "panda_arm1");
+ // if(mgt1.plan(1)) {
+ // //mgt.introspection().publishSolution(*mgt.solutions().front());
+ // mgt1.execute(*mgt1.solutions().front());
+ // }
+
+ std::thread processThread(&MGMediator::processSharedStructure, this);
+
+ // Spin ROS node
+
+ processThread.join();
@@ -252,8 +330,6 @@ moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const
}
return task_;
-
-
}
moveit::task_constructor::Task MGMediator::place(const std::string& obj, const std::string& robotId, const std::string& target){
@@ -566,8 +642,6 @@ void MGMediator::publishTables(){
sendToAll(rss.str(), ss.str());
}
- sendToAll("command/pickup", "");
-
}
void MGMediator::setPanel(){
diff --git a/src/mg_routine.cpp b/src/mg_routine.cpp
index 811cdf5d3b486fa97796c9c99db4a280a3f8c57f..333250c348256f1bd2e18c1667f8f9785565c60c 100644
--- a/src/mg_routine.cpp
+++ b/src/mg_routine.cpp
@@ -18,7 +18,7 @@ int main(int argc, char **argv){
ros::init(argc, argv, "mg_routine");
std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
- ros::AsyncSpinner spinner(1);
+ ros::AsyncSpinner spinner(4);
spinner.start();
std::shared_ptr<MGMediator> mediator = std::make_shared<MGMediator>(n);
@@ -34,6 +34,6 @@ int main(int argc, char **argv){
mediator->mediate();
while (ros::ok()){
- ros::spinOnce();
+ ros::spin();
}
}
\ No newline at end of file