bridge between mtc msg and docker, parallelism should work, try it soon

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 24515
+/home/matteo/reachability/devel/./cmake.lock 25109
 /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

@@ -44,12 +44,17 @@ class Moveit_mediator : public Abstract_mediator{
     std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
     std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_;
     std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
+    ros::Publisher planning_scene_diff_publisher_;
 
 
 
 
     public:
     Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh), sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>()), cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>()),  psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>()){
+        //planning scene publisher
+        planning_scene_diff_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
+
+
         // planner 
         sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
 
@@ -71,7 +76,7 @@ class Moveit_mediator : public Abstract_mediator{
     void publish_tables();
     void load_robot_description();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
-    void parallel_exec(Abstract_robot& penis, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target);
+    void parallel_exec(Abstract_robot& r, moveit::task_constructor::Task& task);
     moveit::task_constructor::Task create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
     inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
 

src/mtc/include/impl/moveit_robot.h

@@ -25,7 +25,6 @@ class Moveit_robot : public Robot{
         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/launch/cell_routine.launch

@@ -8,7 +8,7 @@
 
     <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
+    <rosparam command="load" file="$(find mtc)/descriptions/dummy2.yaml"/>
     
 
     <include file="$(find ceti_double)/launch/demo.launch"> 

src/mtc/src/impl/moveit_mediator.cpp

@@ -136,7 +136,7 @@ void Moveit_mediator::setup_task(){
 		
 	} else {
 		moveit_msgs::CollisionObject bottle1;
-		bottle1.id = "bottle1";
+		bottle1.id = "bottle0";
 		bottle1.header.frame_id = "world";
 		bottle1.header.stamp = ros::Time::now();
 		bottle1.primitives.resize(1);
@@ -157,7 +157,7 @@ void Moveit_mediator::setup_task(){
 		bottle1.operation = bottle1.ADD;
 
 		moveit_msgs::CollisionObject bottle2;
-		bottle2.id = "bottle2";
+		bottle2.id = "bottle1";
 		bottle2.header.frame_id = "world";
 		bottle2.header.stamp = ros::Time::now();
 		bottle2.primitives.resize(1);
@@ -180,27 +180,31 @@ void Moveit_mediator::setup_task(){
 		psi_->applyCollisionObject(bottle1);
 		psi_->applyCollisionObject(bottle2);
 
-		/*
-		for(int j =0; j < objects_[0].size();j++){
-			create_Task(robots_[0], psi_->getObjects().at("bottle1"), objects_[0][j]);
-		}
 
-		for(int j =0; j < objects_[1].size();j++){
-			create_Task(robots_[1], psi_->getObjects().at("bottle2"), objects_[1][j]);
-		}
-		*/
-		//std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(psi_->getObjects().at("bottle1")), std::ref(objects_[0]));
-		//std::thread two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1]));
 
-		//one.join();
-		//two.join();
+		for (int i = 0 ; i < objects_.size(); i++){
+			std::stringstream ss;
+			ss << "bottle" << std::to_string(i);
+			for(int j =0; j < objects_[i].size();j++){
+				moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at(ss.str()), objects_[i][j]);
+				if (task.plan(1)){
+					parallel_exec(std::ref(*robots_[i]), std::ref(task));
+					//std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(task));
+					//one.join();
+				}
+			}
+		}
 	}
 
 }
 
-void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target){
-	for(int j =0; j < target.size();j++){
-		create_Task(&ar, source, target[j]);
+void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit::task_constructor::Task& task){
+	Moveit_robot* mr = dynamic_cast<Moveit_robot*>(&ar);
+	moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg;
+	task.solutions().front()->fillMessage(etsg.solution, &task.introspection());
+	for(auto st : etsg.solution.sub_trajectory){
+		mr->mgi()->execute(st.trajectory);
+		planning_scene_diff_publisher_.publish(st.scene_diff);
 	}
 }