From bd77f55e48730baa30848d5ccaa0fa17ea0bc164 Mon Sep 17 00:00:00 2001 From: KingMaZito <matteo.aneddama@icloud.com> Date: Thu, 17 Nov 2022 14:34:20 +0100 Subject: [PATCH] some changes for docker --- .../profiles/default/devel_collisions.txt | 2 +- src/mtc/include/impl/moveit_mediator.h | 4 +- src/mtc/src/impl/moveit_mediator.cpp | 54 +++++++++++-------- 3 files changed, 36 insertions(+), 24 deletions(-) diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 5a4ae87..2ebf577 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 25109 +/home/matteo/reachability/devel/./cmake.lock 25373 /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 e58276b..7830204 100644 --- a/src/mtc/include/impl/moveit_mediator.h +++ b/src/mtc/include/impl/moveit_mediator.h @@ -76,8 +76,8 @@ class Moveit_mediator : public Abstract_mediator{ void publish_tables(); void load_robot_description(); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); - void parallel_exec(Abstract_robot& r, moveit::task_constructor::Task& task); - moveit::task_constructor::Task create_Task(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); + 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/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index 0a520e4..06ebc61 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -1,6 +1,12 @@ #include "impl/moveit_mediator.h" #include <thread> +#include <mutex> +thread_local moveit::task_constructor::Task task__; +thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_; + +std::mutex task_writing; +std::mutex publish; void Moveit_mediator::publish_tables(){ @@ -124,7 +130,8 @@ void Moveit_mediator::setup_task(){ for( int i = 0; i < objects_.size();i++){ for(int j =0; j < objects_[i].size();j++){ - moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]); + 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()); @@ -180,35 +187,42 @@ void Moveit_mediator::setup_task(){ psi_->applyCollisionObject(bottle1); psi_->applyCollisionObject(bottle2); - + std::vector<std::thread> threads; for (int i = 0 ; i < objects_.size(); i++){ std::stringstream ss; ss << "bottle" << std::to_string(i); - for(int j =0; j < objects_[i].size();j++){ - moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at(ss.str()), objects_[i][j]); - if (task.plan(1)){ - parallel_exec(std::ref(*robots_[i]), std::ref(task)); - //std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(task)); - //one.join(); - } - } + 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])); } - } + for (int i = 0; i < threads.size(); i++) threads[i].join(); + } } + -void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit::task_constructor::Task& task){ - Moveit_robot* mr = dynamic_cast<Moveit_robot*>(&ar); - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg; - task.solutions().front()->fillMessage(etsg.solution, &task.introspection()); - for(auto st : etsg.solution.sub_trajectory){ - mr->mgi()->execute(st.trajectory); - planning_scene_diff_publisher_.publish(st.scene_diff); +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 + } } } -moveit::task_constructor::Task Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){ + +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)); std::string support_surface1 = "nichts"; std::string support_surface2 = "nichts"; @@ -227,8 +241,6 @@ moveit::task_constructor::Task Moveit_mediator::create_Task(Abstract_robot* r, m const std::string object = source.id; moveit::task_constructor::Task task_; - Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r); - std::string name = "Pick&Place"; task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec()))); task_.loadRobotModel(); -- GitLab