diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 2ebf577c9c9a1adbdef98aed3083156d36159f2f..ae68512be7721c72607eab17a3b7a6c2ffe6e34a 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,4 +1,4 @@ /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 diff --git a/src/ceti_double/config/fake_controllers.yaml b/src/ceti_double/config/fake_controllers.yaml index 94406782d8a95546ab9a59c054e72b0559b1d13f..f87c1277b24155a469e55f1ffdbfe3e1780fd86b 100644 --- a/src/ceti_double/config/fake_controllers.yaml +++ b/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 diff --git a/src/ceti_double/config/ompl_planning.yaml b/src/ceti_double/config/ompl_planning.yaml index 0e7e823f52849fb692db55ceb4daea845d94ffb0..eba15e131272fadd88ee70edfd6951f409141b00 100644 --- a/src/ceti_double/config/ompl_planning.yaml +++ b/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 diff --git a/src/ceti_double/config/panda.srdf b/src/ceti_double/config/panda.srdf index c186de1414c57daa88e887d69335303664c68a17..1b03a4e56230235cbd501fb5947cff9b5b68fd89 100644 --- a/src/ceti_double/config/panda.srdf +++ b/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"/> diff --git a/src/ceti_double/config/ros_controllers.yaml b/src/ceti_double/config/ros_controllers.yaml index 2300582b42fd9dbaf75500ad6f5313c3ba36c6fc..cecf7b70216f12e1a240185b5bb0ebfe9eadb520 100644 --- a/src/ceti_double/config/ros_controllers.yaml +++ b/src/ceti_double/config/ros_controllers.yaml @@ -1,124 +1,223 @@ -panda_arm1_controller: - type: effort_controllers/JointTrajectoryController - joints: - - panda_1_joint1 - - panda_1_joint2 - - panda_1_joint3 - - panda_1_joint4 - - panda_1_joint5 - - panda_1_joint6 - - panda_1_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 -hand_1_controller: - type: effort_controllers/JointTrajectoryController - joints: - - panda_1_finger_joint1 - - panda_1_finger_joint2 - gains: - panda_1_finger_joint1: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - panda_1_finger_joint2: - p: 100 - d: 1 - i: 1 - i_clamp: 1 -panda_arm2_controller: - type: effort_controllers/JointTrajectoryController - joints: - - panda_2_joint1 - - panda_2_joint2 - - panda_2_joint3 - - panda_2_joint4 - - panda_2_joint5 - - panda_2_joint6 - - panda_2_joint7 - gains: - 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 -hand_2_controller: - type: effort_controllers/JointTrajectoryController - joints: - - panda_2_finger_joint1 - - panda_2_finger_joint2 - gains: - panda_2_finger_joint1: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - panda_2_finger_joint2: - p: 100 - d: 1 - i: 1 - i_clamp: 1 \ No newline at end of file +controller_list: + panda_arm1_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 + 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 + hand_1_controller: + action_ns: follow_joint_trajectory + type: follow_joint_trajectory + default: true + joints: + - panda_1_finger_joint1 + - panda_1_finger_joint2 + gains: + panda_1_finger_joint1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_1_finger_joint2: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_arm2_controller: + action_ns: follow_joint_trajectory + type: follow_joint_trajectory + default: true + joints: + - panda_2_joint1 + - panda_2_joint2 + - panda_2_joint3 + - panda_2_joint4 + - panda_2_joint5 + - panda_2_joint6 + - panda_2_joint7 + gains: + 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 + hand_2_controller: + action_ns: follow_joint_trajectory + type: follow_joint_trajectory + default: true + joints: + - panda_2_finger_joint1 + - panda_2_finger_joint2 + gains: + panda_2_finger_joint1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_2_finger_joint2: + p: 100 + 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 diff --git a/src/ceti_double/launch/demo.launch b/src/ceti_double/launch/demo.launch index 30e85064d6a46ac3d3a25077f1c4fdde32e7c1df..d2c944e0a4547ecc5a3fb6bbaa3b2ad1cc928919 100644 --- a/src/ceti_double/launch/demo.launch +++ b/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"/> diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h index 783020419e6b977b66b4350c902b9edaf30b276f..84f6d51af1601cf077c98bee762873f98b7b5cc9 100644 --- a/src/mtc/include/impl/moveit_mediator.h +++ b/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_;}; diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch index a808d5e4adfc4c5fde0828bbd04ceee86cf38359..f0dce942a8bc68a1735d2b066f600c480c1824d3 100644 --- a/src/mtc/launch/cell_routine.launch +++ b/src/mtc/launch/cell_routine.launch @@ -9,14 +9,15 @@ <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> + + <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" /> </include> diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index 06ebc614681a248132da462f5d96a2ca6b887f79..6fd6804363daea95a32ee43c2099565c8fea48b2 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/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,39 +184,130 @@ 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); + } + */ + - for (int i = 0; i < threads.size(); i++) threads[i].join(); + 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)); + } + + 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::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(); - - //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 +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; + + 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){ + 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){