Skip to content
Snippets Groups Projects
Commit 53dec5d7 authored by KingMaZito's avatar KingMaZito
Browse files

...

parent 0f15141c
Branches
No related tags found
No related merge requests found
Pipeline #15002 failed
/home/matteo/ws_panda/devel/./cmake.lock 42 /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.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
...@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{
void build_wings(std::bitset<3>& wing,int& robot) override; void build_wings(std::bitset<3>& wing,int& robot) override;
void set_wings(std::vector<std::vector<wing_BP>>& wbp) 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 publish_tables();
void load_robot_description(); void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
......
...@@ -255,11 +255,11 @@ void Moveit_mediator::setup_task(){ ...@@ -255,11 +255,11 @@ void Moveit_mediator::setup_task(){
} }
ROS_INFO("entry names"); ROS_INFO("entry names");
for (auto den : ps1->allowed_collision_matrix.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"); ROS_INFO("entry values");
for (auto den : ps1->allowed_collision_matrix.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"); ROS_INFO("[Robot 1] fft");
for (auto ft : ps1->fixed_frame_transforms){ for (auto ft : ps1->fixed_frame_transforms){
...@@ -325,11 +325,11 @@ void Moveit_mediator::setup_task(){ ...@@ -325,11 +325,11 @@ void Moveit_mediator::setup_task(){
} }
ROS_INFO("entry names"); ROS_INFO("entry names");
for (auto den : ps2->allowed_collision_matrix.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"); ROS_INFO("entry values");
for (auto den : ps2->allowed_collision_matrix.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"); ROS_INFO("[Robot 2] fft");
for (auto ft : ps2->fixed_frame_transforms){ for (auto ft : ps2->fixed_frame_transforms){
...@@ -402,33 +402,8 @@ void Moveit_mediator::setup_task(){ ...@@ -402,33 +402,8 @@ void Moveit_mediator::setup_task(){
} }
void Moveit_mediator::merge_stages(std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>& tasks){ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr){
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){ void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment