debug

src/mediator/mg_mediator.cpp

@@ -28,22 +28,19 @@ void MGMediator::mergeACM(){
 }
 
 void MGMediator::parallelExec(moveit_task_constructor_msgs::SubTrajectory st, const std::string& robotId){
+	AbstractRobotDecorator* mr = nullptr;
 	{
+		mr= robots_.at(robotId).get();
 		ROS_INFO("parallelExec step 1");
 		std::unique_lock<std::mutex> lock(shared_mutex);
-		manipulateACM(st.scene_diff, robotId);
-		mergePS(st.scene_diff, robotId);
+		//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);
+	planning_scene_diff_publisher_->publish(st.scene_diff);
 	ROS_INFO("parallelExec step 4");
 }
 
@@ -51,18 +48,55 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
 	// get full mr link list 
 
 	ROS_INFO("mergePS step 1");
+	AbstractRobotDecorator* mr= robots_.at(robotId).get();
 
 	if (!ps_){
-		ps_ = std::make_shared<moveit_msgs::PlanningScene>(in);
-		ROS_INFO("mergePS step 2");
-		return;
+		ps_ = std::make_shared<moveit_msgs::PlanningScene>();
+
+		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;
+			ps_->is_diff |= in.is_diff;
+			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"]);
+			links.push_back(mr->map()["right_finger"]);
+			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]){
+						ps_->robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
+						ps_->robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
+						ps_->robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
 					}
+				}
+				ROS_INFO("mergePS step 6");
 
-	AbstractRobotDecorator* mr = nullptr;
-	{
-		std::unique_lock<std::mutex> lock(shared_mutex);
-		mr= robots_.at(robotId).get();
+				for(int i = 0; i < in.link_padding.size(); i++){
+					if(link == in.link_padding[i].link_name){
+						ps_->link_padding.push_back(in.link_padding[i]);
+					}
+				}
+
+				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");
+		return;
+	}
+
 	ROS_INFO("mergePS step 3");
 
 	for (auto ao : in.robot_state.attached_collision_objects) ps_->robot_state.attached_collision_objects.push_back(ao);
@@ -179,16 +213,15 @@ void MGMediator::processSharedStructure() {
 	ROS_INFO("processSharedStructure step 6");
 	for(auto& t : th){
 		t.join();
-	
 	}
 
 	ROS_INFO("processSharedStructure step 7");
 
-	mergeACM();
+	//mergeACM();
 
 	ROS_INFO("processSharedStructure step 8");
-	planning_scene_diff_publisher_->publish(ps_.get());
-	
+	//planning_scene_diff_publisher_->publish(*ps_);
+	ps_ = nullptr;
 
 	// The lock is automatically released here when `lock` goes out of scope