docker try

.catkin_tools/profiles/default/devel_collisions.txt

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

src/ceti_double/config/fake_controllers.yaml

@@ -29,6 +29,23 @@ controller_list:
     joints:
       - panda_2_finger_joint1
       - panda_2_finger_joint2
+  - name: fake_panda_arms_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - panda_1_joint1
+      - panda_1_joint2
+      - panda_1_joint3
+      - panda_1_joint4
+      - panda_1_joint5
+      - panda_1_joint6
+      - panda_1_joint7
+      - panda_2_joint1
+      - panda_2_joint2
+      - panda_2_joint3
+      - panda_2_joint4
+      - panda_2_joint5
+      - panda_2_joint6
+      - panda_2_joint7
 initial:  # Define initial robot poses per group
   - group: panda_arm1
     pose: ready

src/ceti_double/config/ompl_planning.yaml

@@ -231,3 +231,31 @@ hand_2:
     - LazyPRMstar
     - SPARS
     - SPARStwo
+panda_arms:
+  planner_configs:
+    - AnytimePathShortening
+    - SBL
+    - EST
+    - LBKPIECE
+    - BKPIECE
+    - KPIECE
+    - RRT
+    - RRTConnect
+    - RRTstar
+    - TRRT
+    - PRM
+    - PRMstar
+    - FMT
+    - BFMT
+    - PDST
+    - STRIDE
+    - BiTRRT
+    - LBTRRT
+    - BiEST
+    - ProjEST
+    - LazyPRM
+    - LazyPRMstar
+    - SPARS
+    - SPARStwo
+  projection_evaluator: joints(panda_1_joint1,panda_1_joint2)
+  longest_valid_segment_fraction: 0.005

src/ceti_double/config/panda.srdf

@@ -25,6 +25,10 @@
         <link name="panda_2_leftfinger"/>
         <link name="panda_2_rightfinger"/>
     </group>
+    <group name="panda_arms">
+        <group name="panda_arm1"/>
+        <group name="panda_arm2"/>
+    </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="ready" group="panda_arm1">
         <joint name="panda_1_joint1" value="0"/>

src/ceti_double/config/ros_controllers.yaml

+controller_list:
   panda_arm1_controller:
-  type: effort_controllers/JointTrajectoryController
+    action_ns: follow_joint_trajectory
+    type: follow_joint_trajectory
+    default: true
     joints:
       - panda_1_joint1
       - panda_1_joint2
@@ -45,7 +48,9 @@ panda_arm1_controller:
         i: 1
         i_clamp: 1
   hand_1_controller:
-  type: effort_controllers/JointTrajectoryController
+    action_ns: follow_joint_trajectory
+    type: follow_joint_trajectory
+    default: true
     joints:
       - panda_1_finger_joint1
       - panda_1_finger_joint2
@@ -61,7 +66,9 @@ hand_1_controller:
         i: 1
         i_clamp: 1
   panda_arm2_controller:
-  type: effort_controllers/JointTrajectoryController
+    action_ns: follow_joint_trajectory
+    type: follow_joint_trajectory
+    default: true
     joints:
       - panda_2_joint1
       - panda_2_joint2
@@ -107,7 +114,9 @@ panda_arm2_controller:
         i: 1
         i_clamp: 1
   hand_2_controller:
-  type: effort_controllers/JointTrajectoryController
+    action_ns: follow_joint_trajectory
+    type: follow_joint_trajectory
+    default: true    
     joints:
       - panda_2_finger_joint1
       - panda_2_finger_joint2
@@ -122,3 +131,93 @@ hand_2_controller:
         d: 1
         i: 1
         i_clamp: 1
+  panda_arms_controller:
+    action_ns: follow_joint_trajectory
+    type: follow_joint_trajectory
+    default: true
+    joints:
+      - panda_1_joint1
+      - panda_1_joint2
+      - panda_1_joint3
+      - panda_1_joint4
+      - panda_1_joint5
+      - panda_1_joint6
+      - panda_1_joint7
+      - panda_2_joint1
+      - panda_2_joint2
+      - panda_2_joint3
+      - panda_2_joint4
+      - panda_2_joint5
+      - panda_2_joint6
+      - panda_2_joint7
+    gains:
+      panda_1_joint1:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_1_joint2:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_1_joint3:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_1_joint4:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_1_joint5:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_1_joint6:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_1_joint7:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_2_joint1:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_2_joint2:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_2_joint3:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_2_joint4:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_2_joint5:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_2_joint6:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
+      panda_2_joint7:
+        p: 100
+        d: 1
+        i: 1
+        i_clamp: 1
\ No newline at end of file

src/ceti_double/launch/demo.launch

@@ -56,7 +56,6 @@
     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
   </include>
 
-
   <!-- Run Rviz and load the default config to see the state of the move_group node
   <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
     <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>

src/mtc/include/impl/moveit_mediator.h

@@ -32,6 +32,12 @@
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 #include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
 #include <eigen_conversions/eigen_msg.h>
+#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
+#include <moveit/planning_scene_monitor/current_state_monitor.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/moveit_cpp/moveit_cpp.h>
+
+
 
 class Moveit_mediator : public Abstract_mediator{
     protected:
@@ -45,16 +51,33 @@ class Moveit_mediator : public Abstract_mediator{
     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_;
+    //trajectory_execution_manager::TrajectoryExecutionManager* tem_;
+    moveit::planning_interface::MoveGroupInterface* mgi_;
+    std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_ptr_;
+
 
 
 
 
     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>()){
+        mgi_ = new moveit::planning_interface::MoveGroupInterface("panda_arms");
+        moveit_cpp_ptr_ = std::make_shared<moveit_cpp::MoveItCpp>(nh_);
+        
+        /*
+        ROS_INFO("1");
+        planning_scene_monitor::PlanningSceneMonitorPtr psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
+        ROS_INFO("2");
+        psm_->startSceneMonitor("/move_group/monitored_planning_scene");
+        ROS_INFO("3");
+        tem_ = new trajectory_execution_manager::TrajectoryExecutionManager(mgi.getRobotModel(), psm_->getStateMonitor());
+        ROS_INFO("4");
+        */
+
+        
         //planning scene publisher
         planning_scene_diff_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
 
-
         // planner 
         sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
 
@@ -72,11 +95,11 @@ 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 publish_tables();
     void load_robot_description();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
-    void parallel_exec(Moveit_robot& r, moveit_msgs::CollisionObject source,  std::vector<tf2::Transform> target);
+    void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
     moveit::task_constructor::Task create_Task(Moveit_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/launch/cell_routine.launch

@@ -9,13 +9,14 @@
     <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/dummy2.yaml"/>
-    
+    <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
 
     <include file="$(find ceti_double)/launch/demo.launch"> 
         <arg name="use_rviz" value="false"/>
     </include> 
-    <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
     
+    <include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+    <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
 
     <include file="$(find ceti_double)/launch/moveit_rviz.launch">
         <arg name="rviz_config" value="$(find mtc)/launch/rviz/cell_routine.rviz" />

src/mtc/src/impl/moveit_mediator.cpp

@@ -6,7 +6,7 @@ thread_local moveit::task_constructor::Task task__;
 thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
 
 std::mutex task_writing;
-std::mutex publish;
+//std::mutex publish;
 
 
 void Moveit_mediator::publish_tables(){
@@ -133,14 +133,11 @@ void Moveit_mediator::setup_task(){
 				Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
 				moveit::task_constructor::Task task = create_Task(mr, psi_->getObjects().at("bottle"), objects_[i][j]);
 				if (task.plan(1)){
-					moveit_msgs::MoveItErrorCodes execute_result;
-					execute_result = task.execute(*task.solutions().front());
-					//task_.introspection().publishSolution(*task_.solutions().front());
+					task.execute(*task.solutions().front());
 				}
 				
 			}
 		}
-		
 	} else {
 		moveit_msgs::CollisionObject bottle1;
 		bottle1.id = "bottle0";
@@ -154,8 +151,8 @@ void Moveit_mediator::setup_task(){
 		bottle1.primitives[0].dimensions[2] = box_size.getZ();
 
 		bottle1.primitive_poses.resize(1);
-		bottle1.primitive_poses[0].position.x = -0.3f;
-		bottle1.primitive_poses[0].position.y = -0.6f;
+		bottle1.primitive_poses[0].position.x = objects_[0][0].getOrigin().getX();
+		bottle1.primitive_poses[0].position.y = objects_[0][0].getOrigin().getY();
 		bottle1.primitive_poses[0].position.z = 0.9355f;
 		bottle1.primitive_poses[0].orientation.x = 0;
 		bottle1.primitive_poses[0].orientation.y = 0;
@@ -175,8 +172,8 @@ void Moveit_mediator::setup_task(){
 		bottle2.primitives[0].dimensions[2] = box_size.getZ();
 
 		bottle2.primitive_poses.resize(1);
-		bottle2.primitive_poses[0].position.x = 0.1f; 
-		bottle2.primitive_poses[0].position.y = 1.01f;
+		bottle2.primitive_poses[0].position.x = objects_[1][0].getOrigin().getX(); 
+		bottle2.primitive_poses[0].position.y = objects_[1][0].getOrigin().getY();
 		bottle2.primitive_poses[0].position.z = 0.9355f;
 		bottle2.primitive_poses[0].orientation.x = 0;
 		bottle2.primitive_poses[0].orientation.y = 0;
@@ -187,40 +184,131 @@ void Moveit_mediator::setup_task(){
 		psi_->applyCollisionObject(bottle1);
 		psi_->applyCollisionObject(bottle2);
 
-		std::vector<std::thread> threads;
+		std::vector<std::thread> ths;
+		std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> tasks;
 
 		for (int i = 0 ; i < objects_.size(); i++){
 			std::stringstream ss;
 			ss << "bottle" << std::to_string(i);
 			Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
-			threads.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
+			std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
+			for (int j = 1; j < objects_[i].size(); j++){
+				moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(ss.str()), objects_[i][j]);
+				if(mgt.plan(1)) {
+					moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
+					mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
+					tasks_per_robot.push(e);
+
+					moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
+					temp.operation = temp.MOVE;
+					temp.pose.position.x = objects_[i][j].getOrigin().getX();
+					temp.pose.position.y = objects_[i][j].getOrigin().getY();
+					psi_->applyCollisionObject(temp);
+				}		
+			}
+
+			moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
+			temp.operation = temp.MOVE;
+			temp.pose.position.x = objects_[i][0].getOrigin().getX();
+			temp.pose.position.y = objects_[i][0].getOrigin().getY();
+			psi_->applyCollisionObject(temp);
+			tasks.push_back(tasks_per_robot);
+			//ths.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
+		}
+		/*
+		//for (int i = 0; i < ths.size(); i++) ths[i].join();
+		std::vector<std::vector<std::vector<moveit_msgs::RobotTrajectory>>> rt_r_t;
+		for (int i =0; i < tasks.size(); i++){
+			std::vector<std::vector<moveit_msgs::RobotTrajectory>> rt_r;
+			while (!tasks[i].empty()){
+				std::vector<moveit_msgs::RobotTrajectory> rt;
+				for(auto sub_t : tasks[i].front().solution.sub_trajectory){
+					rt.push_back(sub_t.trajectory);
+				}
+				tasks[i].pop();
+				rt_r.push_back(rt);
+			}
+			rt_r_t.push_back(rt_r);
+		} 
+		*/
+
+
+		while (!tasks[0].empty() && !tasks[1].empty()){
+			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* t1 = nullptr;
+				moveit_msgs::RobotTrajectory* t2 = nullptr;
+				moveit_msgs::PlanningScene* ps1 = nullptr;
+				moveit_msgs::PlanningScene* ps2 = nullptr;
+
+				if (i < tasks[0].front().solution.sub_trajectory.size()){
+					t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
+					ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff;
+				} 
+
+				if (i < tasks[1].front().solution.sub_trajectory.size()){
+					t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
+					ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
+
+				}
+
+				std::vector<std::thread> th;
+
+				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));
 				}
 
-		for (int i = 0; i < threads.size(); i++) threads[i].join();
+				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));				}
+
+				for(int i = 0; i < th.size(); i++){
+					th[i].join();
+				}
+			}
+			tasks[0].pop();
+			tasks[1].pop();
 		}
 	}
 
+}
+
+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::parallel_exec(Moveit_robot& mr, moveit_msgs::CollisionObject source,  std::vector<tf2::Transform> target){
-	ros::Duration sleep(1.0);
-	for(int i = 0; i < target.size(); i++){
-		//task writing task
-		task_writing.lock();
-		task__ = create_Task(&mr, source, target[i]);
-		if (!task__.plan(1)) { continue; task_writing.unlock();}
-		task__.solutions().front()->fillMessage(etsg_.solution, &task__.introspection());
-		task_writing.unlock();
+		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();
+		}
 
-		//Iterate throug trajetory
-		for(int j = 0; j <  etsg_.solution.sub_trajectory.size(); j++){
-			mr.mgi()->execute(etsg_.solution.sub_trajectory[j].trajectory);
-			sleep.sleep();
-			planning_scene_diff_publisher_.publish(etsg_.solution.sub_trajectory[j].scene_diff);
-			// secure publishing
+		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){
+	mr.mgi()->execute(rt);
+	planning_scene_diff_publisher_.publish(ps);
+
+}
+
+
 
 moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, moveit_msgs::CollisionObject& source, tf2::Transform& target){
     tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));