...

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 26266
+/home/matteo/reachability/devel/./cmake.lock 26279
 /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_stages(std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>& tasks);
+    void merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, 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/src/impl/moveit_mediator.cpp

@@ -255,11 +255,11 @@ void Moveit_mediator::setup_task(){
 					}
 					ROS_INFO("entry names");
 					for (auto  den : ps1->allowed_collision_matrix.entry_names){
-						ROS_INFO("%i", den);
+						ROS_INFO("%i", den.c_str());
 					}					
 					ROS_INFO("entry values");
 					for (auto  den : ps1->allowed_collision_matrix.entry_values){
-						ROS_INFO("%i", den);
+						for (auto ene : den.enabled) ROS_INFO("%i", ene);
 					}
 					ROS_INFO("[Robot 1] fft");
 					for (auto  ft : ps1->fixed_frame_transforms){
@@ -325,11 +325,11 @@ void Moveit_mediator::setup_task(){
 					}
 					ROS_INFO("entry names");
 					for (auto  den : ps2->allowed_collision_matrix.entry_names){
-						ROS_INFO("%i", den);
+						ROS_INFO("%i", den.c_str());
 					}					
 					ROS_INFO("entry values");
 					for (auto  den : ps2->allowed_collision_matrix.entry_values){
-						ROS_INFO("%i", den);
+						for (auto ene : den.enabled) ROS_INFO("%i", ene);
 					}
 					ROS_INFO("[Robot 2] fft");
 					for (auto  ft : ps2->fixed_frame_transforms){
@@ -402,33 +402,8 @@ void Moveit_mediator::setup_task(){
 
 }
 
-void Moveit_mediator::merge_stages(std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>& tasks){
-	for(int i = 0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++) {
-		moveit_msgs::RobotTrajectory jt1;
-		moveit_msgs::RobotTrajectory jt2;
+void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr){
 	
-		if ( i < tasks[0].front().solution.sub_trajectory.size()) {
-			jt1.joint_trajectory.header = tasks[0].front().solution.sub_trajectory[i].trajectory.joint_trajectory.header;
-			for(auto jn : tasks[0].front().solution.sub_trajectory[i].trajectory.joint_trajectory.joint_names) jt1.joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[0].front().solution.sub_trajectory[i].trajectory.joint_trajectory.points) jt1.joint_trajectory.points.push_back(point);
-			jt1.multi_dof_joint_trajectory.header = tasks[0].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.header;
-			for(auto jn : tasks[0].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.joint_names) jt1.multi_dof_joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[0].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.points) jt1.multi_dof_joint_trajectory.points.push_back(point);
-			tasks[0].pop();
-		}
-
-		if ( i < tasks[1].front().solution.sub_trajectory.size()) {
-			jt2.joint_trajectory.header = tasks[1].front().solution.sub_trajectory[i].trajectory.joint_trajectory.header;
-			jt2.multi_dof_joint_trajectory.header = tasks[1].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.header;
-			for(auto jn : tasks[1].front().solution.sub_trajectory[i].trajectory.joint_trajectory.joint_names) jt2.joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[1].front().solution.sub_trajectory[i].trajectory.joint_trajectory.points) jt2.joint_trajectory.points.push_back(point);
-			for(auto jn : tasks[1].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.joint_names) jt2.multi_dof_joint_trajectory.joint_names.push_back(jn);
-			for(auto point : tasks[1].front().solution.sub_trajectory[i].trajectory.multi_dof_joint_trajectory.points) jt2.multi_dof_joint_trajectory.points.push_back(point);
-			tasks[1].pop();
-		}
-		mgi_->execute(jt1);
-		mgi_->asyncExecute(jt2);
-	}
 }
 	
 void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){