...

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 26344
+/home/matteo/reachability/devel/./cmake.lock 26357
 /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& origin, moveit_msgs::PlanningScene* merge, 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

@@ -24,7 +24,6 @@ class Moveit_robot : public Robot{
         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()));
-
     }
 
     inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}

src/mtc/src/impl/moveit_mediator.cpp

 #include "impl/moveit_mediator.h"
 #include <thread>
 #include <mutex> 
+#include <algorithm>
 
 thread_local moveit::task_constructor::Task task__;
 thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
@@ -305,6 +306,13 @@ void Moveit_mediator::setup_task(){
 						for (auto p : aco.touch_links) ROS_INFO("aco touch link  %s", p.c_str());
 						ROS_INFO("aco collis obj weight %d", aco.weight);
 					}
+
+					for (auto d : ps1->robot_state.joint_state.position) ROS_INFO("js position %d ", d);
+					for (auto d : ps1->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
+					for (auto d : ps1->robot_state.joint_state.velocity) ROS_INFO(" js vel %d", d);
+					for (auto d : ps1->robot_state.joint_state.effort) ROS_INFO(" js e %d", d);
+					ROS_INFO ("%s, %i", ps1->robot_state.joint_state.header.frame_id.c_str(), p1->robot_state.joint_state.header.seq);
+
 					for (auto p : ps1->world.collision_objects) ROS_INFO("world co  id %s", p.id);
 					ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps1->world.octomap.origin.position.x, ps1->world.octomap.origin.position.y, ps1->world.octomap.origin.position.z, ps1->world.octomap.origin.orientation.x, ps1->world.octomap.origin.orientation.y, ps1->world.octomap.origin.orientation.z, ps1->world.octomap.origin.orientation.w);
 					ROS_INFO("world om header time, id %s, seq %i", ps1->world.octomap.header.frame_id.c_str(), ps1->world.octomap.header.seq);
@@ -379,7 +387,15 @@ void Moveit_mediator::setup_task(){
 						for (auto p : aco.touch_links) ROS_INFO("aco touch link  %s", p.c_str());
 						ROS_INFO("aco collis obj weight %d", aco.weight);
 					}
-					for (auto p : ps2->world.collision_objects) ROS_INFO("world co  id %s", p.id);
+					for (auto d : ps2->robot_state.joint_state.position) ROS_INFO("js position %d ", d);
+					for (auto d : ps2->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
+					for (auto d : ps2->robot_state.joint_state.velocity) ROS_INFO(" js vel %d", d);
+					for (auto d : ps2->robot_state.joint_state.effort) ROS_INFO(" js e %d", d);
+					ROS_INFO ("%s, %i", ps2->robot_state.joint_state.header.frame_id.c_str(), ps2->robot_state.joint_state.header.seq);
+
+
+					ROS_INFO("rs js id %s", ps2->robot_state.joint_state.position);
+					for (auto p : ps2->world.collision_objects) ROS_INFO("world co  id %s", p.id.c_str());
 					ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps2->world.octomap.origin.position.x, ps2->world.octomap.origin.position.y, ps2->world.octomap.origin.position.z, ps2->world.octomap.origin.orientation.x, ps2->world.octomap.origin.orientation.y, ps2->world.octomap.origin.orientation.z, ps2->world.octomap.origin.orientation.w);
 					ROS_INFO("world om header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq);
 					ROS_INFO("world om Data");
@@ -408,11 +424,24 @@ void Moveit_mediator::setup_task(){
 
 }
 
-void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr){
-	for (auto acm : merge->allowed_collision_matrix.default_entry_names){
-			// dafd
+void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
+	/*
+	for( auto aco : in->robot_state.attached_collision_objects) {
+		out.robot_state.attached_collision_objects.push_back(aco);
+	}
+
+	for( auto aco : in->robot_state.joint_state) {
+		out.robot_state.attached_collision_objects.push_back(aco);
 	}
+	sensor_msgs::JointState::
 
+	for (auto str : res.allowed_collision_matrix.entry_names){
+		auto entry = std::find(origin->allowed_collision_matrix.entry_names.begin(), origin->allowed_collision_matrix.entry_names.end(), str);
+		if (entry != origin->allowed_collision_matrix.entry_names.end()){
+			origin->allowed_collision_matrix.entry_values[entry - origin->allowed_collision_matrix.entry_names.begin()]
+		}
+	}
+	*/
 }
 	
 void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){