...

src/mediator/mg_mediator.cpp

@@ -11,35 +11,50 @@ void MGMediator::mergeACM(){
 	moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
 	acmt.entry_values.resize(acm_.size());
 
+	ROS_INFO("mergeACM step 1");
+
 	int i = 0;
 	for (auto& a : acm_){
 		acmt.entry_names.push_back(a.first);
 		acmt.entry_values[i].enabled = a.second;
 		i++;
 	}
+	ROS_INFO("mergeACM step 2");
 
 	ps_->allowed_collision_matrix = acmt;
+		
+	ROS_INFO("mergeACM step 3");
+
 }
 
 void MGMediator::parallelExec(moveit_task_constructor_msgs::SubTrajectory st, const std::string& robotId){
 	{
+		ROS_INFO("parallelExec step 1");
 		std::unique_lock<std::mutex> lock(shared_mutex);
 		manipulateACM(st.scene_diff, robotId);
 		mergePS(st.scene_diff, robotId);
+		ROS_INFO("parallelExec step 2");
+
 	}
 
 	AbstractRobotDecorator* mr = nullptr;
 	{
 		std::unique_lock<std::mutex> lock(shared_mutex);
 		mr= robots_.at(robotId).get();
+		ROS_INFO("parallelExec step 3");
 	}
 	mr->mgi()->execute(st.trajectory);
+	ROS_INFO("parallelExec step 4");
 }
 
 void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string& robotId){
 	// get full mr link list 
+
+	ROS_INFO("mergePS step 1");
+
 	if (!ps_){
 		ps_ = std::make_shared<moveit_msgs::PlanningScene>(in);
+		ROS_INFO("mergePS step 2");
 		return;
 	}
 
@@ -48,6 +63,7 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
 		std::unique_lock<std::mutex> lock(shared_mutex);
 		mr= robots_.at(robotId).get();
 	}
+	ROS_INFO("mergePS step 3");
 
 	for (auto ao : in.robot_state.attached_collision_objects) ps_->robot_state.attached_collision_objects.push_back(ao);
 	ps_->robot_state.is_diff |= in.robot_state.is_diff;
@@ -55,6 +71,7 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
 	ps_->robot_state.joint_state.header = in.robot_state.joint_state.header;
 	ps_->robot_model_name = "panda";
 
+	ROS_INFO("mergePS step 4");
 	std::vector<std::string> links;
 	for (auto link : mr->mgi()->getLinkNames()) links.push_back(link);
 	links.push_back(mr->map()["left_finger"]);
@@ -62,6 +79,7 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
 	links.push_back(mr->map()["hand_link"]);
 	links.push_back(mr->map()["base"]);
 
+	ROS_INFO("mergePS step 5");
 	for (auto link : links) {
 		for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
 			if(link == in.robot_state.joint_state.name[i]){
@@ -70,6 +88,7 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
 				ps_->robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
 			}
 		}
+	ROS_INFO("mergePS step 6");
 
 		for(int i = 0; i < in.link_padding.size(); i++){
 			if(link == in.link_padding[i].link_name){
@@ -77,24 +96,30 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
 			}
 		}
 
+	ROS_INFO("mergePS step 7");
+
 		for(int i = 0; i < in.link_scale.size(); i++){
 			if(link == in.link_scale[i].link_name){
 				ps_->link_scale.push_back(in.link_scale[i]);
 			}
 		}
 	}	
+		
+	ROS_INFO("mergePS step 8");
+
 }
 
 
 void MGMediator::processSharedStructure() {
   while (ros::ok()) {
 	std::vector<std::pair<std::string, moveit_task_constructor_msgs::SubTrajectory>> peak;
-
+	ROS_INFO("processSharedStructure step 1");
 	{
 		std::unique_lock<std::mutex> lock(shared_mutex);
 
 		// Wait until the shared structure is not empty
 		cv.wait(lock, []{ return !shared_structure.empty(); });
+		ROS_INFO("processSharedStructure step 2");
 
 		// Iterate over the vectors in shared_structure
 		for (auto it = shared_structure.begin(); it != shared_structure.end(); ++it) {
@@ -102,6 +127,7 @@ void MGMediator::processSharedStructure() {
 			// Get the first entry of the vector and add it to peak
 			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());
+			ROS_INFO("processSharedStructure step 3");
 			}
 
 			if (it->second.solution.sub_trajectory.empty()) {
@@ -110,7 +136,7 @@ void MGMediator::processSharedStructure() {
 					But I want to publish only vanished objects
 					TODO: Implement your own stage, so you don't have to do this embarrassing loops
 				*/
-
+				ROS_INFO("processSharedStructure step 4");
 				std::string temp;
 				try{
 					for (const auto& c : cuboid_reader_->cuboidBox()){
@@ -143,18 +169,25 @@ void MGMediator::processSharedStructure() {
 
 	}
 
+	ROS_INFO("processSharedStructure step 5");
+
 	std::vector<std::thread> th;
 	for (const auto& exec : peak){
 		th.push_back(std::thread(&MGMediator::parallelExec, this, exec.second, std::ref(exec.first)));
 	}
 
+	ROS_INFO("processSharedStructure step 6");
 	for(auto& t : th){
 		t.join();
 	
 	}
 
+	ROS_INFO("processSharedStructure step 7");
+
 	mergeACM();
-	planning_scene_diff_publisher_->publish(*ps_);
+
+	ROS_INFO("processSharedStructure step 8");
+	planning_scene_diff_publisher_->publish(ps_.get());
 	
 
 	// The lock is automatically released here when `lock` goes out of scope
@@ -166,7 +199,7 @@ void MGMediator::processSharedStructure() {
 	// 	}
 	// }
 
-	ROS_INFO("Do something");
+	ROS_INFO("processSharedStructure step 9");
 
 	peak.clear();
 
@@ -333,7 +366,8 @@ void MGMediator::mediate(){
 	// 	//mgt.introspection().publishSolution(*mgt.solutions().front());
 	// 	mgt1.execute(*mgt1.solutions().front());
 	// }
-
+	// Setup shared ACM
+	for(auto& a: acm_) a.second.resize(acm_.size(), 0);
 	std::thread processThread(&MGMediator::processSharedStructure, this);
 
 	// Spin ROS node