...

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 26396
+/home/matteo/reachability/devel/./cmake.lock 26448
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79

src/mtc/include/impl/moveit_mediator.h

@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{
     void build_wings(std::bitset<3>& wing,int& robot) override;
     void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
 
-    void merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
+    void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
     void publish_tables();
     void load_robot_description();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);

src/mtc/include/impl/moveit_robot.h

@@ -19,8 +19,11 @@ class Moveit_robot : public Robot{
         std::stringstream hand_n, ik_frame_n, name_n;
         hand_n << "hand_" << name.back();
         ik_frame_n << "panda_" << name.back() << "_link8";
+        auto mgi_hand = moveit::planning_interface::MoveGroupInterface(hand_n.str());
 
-
+        map_.insert(std::make_pair<std::string, std::string>("right_finger", mgi_hand.getLinkNames()[0].c_str()));
+        map_.insert(std::make_pair<std::string, std::string>("left_finger", mgi_hand.getLinkNames()[1].c_str()));
+        map_.insert(std::make_pair<std::string, std::string>("hand_link", mgi_hand.getLinkNames()[2].c_str()));
         map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
         map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
         map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));

src/mtc/src/impl/moveit_mediator.cpp

@@ -240,6 +240,7 @@ void Moveit_mediator::setup_task(){
 				moveit_msgs::RobotTrajectory* t2 = nullptr;
 				moveit_msgs::PlanningScene* ps1 = nullptr;
 				moveit_msgs::PlanningScene* ps2 = nullptr;
+				moveit_msgs::PlanningScene ps_m;
 
 				if (i < tasks[0].front().solution.sub_trajectory.size()){
 					t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
@@ -325,6 +326,7 @@ void Moveit_mediator::setup_task(){
 					ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
 
 
+					
 					ROS_INFO("[Robot 2] acm");
 					ROS_INFO("default entry names");
 					for (auto  den : ps2->allowed_collision_matrix.default_entry_names){
@@ -406,15 +408,22 @@ void Moveit_mediator::setup_task(){
 				if (t1){
 					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
 					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
+					merge_ps(ps_m, ps1, mr);
+
+
 				}
 
 				if (t2){
 					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
-					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));				}
+					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));		
+					merge_ps(ps_m, ps2, mr);
+				}
 
 				for(int i = 0; i < th.size(); i++){
 					th[i].join();
 				}
+				planning_scene_diff_publisher_.publish(ps_m);
+
 			}
 			tasks[0].pop();
 			tasks[1].pop();
@@ -423,7 +432,48 @@ void Moveit_mediator::setup_task(){
 
 }
 
-void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
+void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
+	// get full mr link list 
+	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"]);
+
+
+	for (auto ao : in->robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
+	if (in->robot_state.is_diff) out.robot_state.is_diff = in->robot_state.is_diff;
+	if (in->is_diff) out.is_diff = in->is_diff;
+	out.robot_state.joint_state.header = in->robot_state.joint_state.header;
+	out.robot_model_name = "panda";
+
+
+
+	for (auto link : links) {
+		for(int i = 0; i < in->robot_state.joint_state.name.size(); i++){
+			if(link.c_str() == in->robot_state.joint_state.name[i].c_str()){
+				out.robot_state.joint_state.effort.push_back(in->robot_state.joint_state.effort[i]);
+				out.robot_state.joint_state.position.push_back(in->robot_state.joint_state.position[i]);
+				out.robot_state.joint_state.velocity.push_back(in->robot_state.joint_state.velocity[i]);
+				ROS_INFO("check");
+			}
+		}
+
+		for(int i = 0; i < in->link_padding.size(); i++){
+			if(link.c_str() == in->link_padding[i].link_name.c_str()){
+				out.link_padding.push_back(in->link_padding[i]);
+			}
+		}
+
+		for(int i = 0; i < in->link_scale.size(); i++){
+			if(link.c_str() == in->link_scale[i].link_name.c_str()){
+				out.link_scale.push_back(in->link_scale[i]);
+			}
+		}
+	}
+	
+
 	/*
 	for( auto aco : in->robot_state.attached_collision_objects) {
 		out.robot_state.attached_collision_objects.push_back(aco);
@@ -445,8 +495,6 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::Plan
 	
 void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
 	mr.mgi()->execute(rt);
-	// planning_scene_diff_publisher_.publish(ps);
-
 }