From 53dec5d7ee010e4ebdbd22a7a46862d5398dbedb Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Sun, 20 Nov 2022 17:17:49 +0100
Subject: [PATCH] ...
---
.../profiles/default/devel_collisions.txt | 2 +-
src/mtc/include/impl/moveit_mediator.h | 2 +-
src/mtc/src/impl/moveit_mediator.cpp | 37 +++----------------
3 files changed, 8 insertions(+), 33 deletions(-)
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 14474ed..aa368ea 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 84f6d51..7a0fa09 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 a97854c..35d47b6 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){
--
GitLab