diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 14474ed439c588e89f084c601a0a9e7af47b8f07..aa368eac912fc51e03fb4a4b2e9abdb298e11ec1 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 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 diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h index 84f6d51af1601cf077c98bee762873f98b7b5cc9..7a0fa096f8fdeb55f00082f23f908482e3ea2799 100644 --- a/src/mtc/include/impl/moveit_mediator.h +++ b/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); diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index a97854ca964b7a1a829fcee4581a8d3e2b5bbf99..35d47b6b2305d1a3119760bbb560707ec3dd2e6d 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/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; - - 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::merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr){ + } void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){