Build producer/consumer scenario based on mqtt msg and TC Solutions

.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",

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

src/mediator/mg_mediator.cpp

 #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,13 +56,29 @@ 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(){

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