diff --git a/include/bt/execution.h b/include/bt/execution.h index ef31a31c007360c5c084332192d3d2600ebe550e..913d60c8b2b5a44e9aa693b6a546c3e75d258fba 100644 --- a/include/bt/execution.h +++ b/include/bt/execution.h @@ -23,13 +23,13 @@ class Execution : public BT::StatefulActionNode{ moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_; Moveit_robot* mr_reference_; ros::Publisher* publisher_reference_; - Moveit_mediator* mediator_reference_; + std::map<std::string, moveit_msgs::RobotTrajectory>* executions_; std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_; public: Execution(const std::string& name, const BT::NodeConfiguration& config); - void init(Moveit_mediator* mediator_reference, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets); + void init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions_, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets); inline static BT::PortsList providedPorts() { return {}; } diff --git a/include/impl/moveit_mediator.h b/include/impl/moveit_mediator.h index b1d24493baf421f9f14dfcc8abd45c377ebfcfc3..fa37d5970ec5b3c8b4c8a5167c97ea292ab61dcd 100644 --- a/include/impl/moveit_mediator.h +++ b/include/impl/moveit_mediator.h @@ -77,18 +77,15 @@ class Moveit_mediator : public Abstract_mediator{ std::map<std::string, std::vector<uint8_t>> acm_; std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_; + std::map<std::string, moveit_msgs::RobotTrajectory> executions_; - std::vector<std::thread*> executions_; - - public: Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh); void setup_task(); inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}; - inline std::vector<std::thread*>& executions(){return executions_;}; bool check_collision(const int& robot) override; void mediate() override; @@ -105,11 +102,13 @@ class Moveit_mediator : public Abstract_mediator{ void publish_tables(); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); - // void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt); - void parallel_exec(); + void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt); + //void parallel_exec(); moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); + inline std::map<std::string, moveit_msgs::RobotTrajectory>& executions(){return executions_;}; inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;}; + inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;}; diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp index 2b64e7fcb98f6120a6b849205d7ee5b4249ac53e..eb95195b812b63bfb9e82b1f6ff2c49e1c57a3b2 100644 --- a/src/bt/execution.cpp +++ b/src/bt/execution.cpp @@ -22,21 +22,7 @@ BT::NodeStatus Execution::tick() { BT::NodeStatus Execution::onStart(){ if (it_ != ets_.solution.sub_trajectory.end()){ - //mr_reference_->execute(it_->trajectory); - ROS_INFO("start"); - // std::thread t(&Moveit_mediator::parallel_exec, mediator_reference_); - // if(t.joinable())t.join(); - // mediator_reference_->parallel_exec(); - // mediator_reference_->executions().push_back(std::thread(&Moveit_mediator::parallel_exec, mediator_reference_, std::ref(*mr_reference_), it_->trajectory)); - mediator_reference_->executions().push_back(new std::thread(&Moveit_mediator::parallel_exec, mediator_reference_)); - for (int i = 0; i < mediator_reference_->executions().size();i++){ - if(mediator_reference_->executions()[i]->joinable()) mediator_reference_->executions()[i]->join(); - } - - //publisher_reference_->publish(it_->scene_diff); - ROS_INFO("%i", mediator_reference_->executions().size()); - ROS_INFO("after join in start"); - + executions_->insert(std::pair<std::string, moveit_msgs::RobotTrajectory>(mr_reference_->name(), it_->trajectory)); it_++; return BT::NodeStatus::RUNNING; } else { @@ -48,16 +34,7 @@ BT::NodeStatus Execution::onStart(){ // this method until it return something different from RUNNING BT::NodeStatus Execution::onRunning(){ if (it_ != ets_.solution.sub_trajectory.end()){ - //mgi_reference_->execute(it_->trajectory); - //mediator_reference_->parallel_exec(); - ROS_INFO("running"); - //std::thread t(&Moveit_mediator::parallel_exec, mediator_reference_); - // if(t.joinable())t.join(); - mediator_reference_->executions().push_back(new std::thread(&Moveit_mediator::parallel_exec, mediator_reference_)); - ROS_INFO("after join in running"); - //mediator_reference_->executions().push_back(std::thread(&Moveit_mediator::parallel_exec, mediator_reference_)); - - //publisher_reference_->publish(it_->scene_diff); + executions_->insert(std::pair<std::string, moveit_msgs::RobotTrajectory>(mr_reference_->name(), it_->trajectory)); it_++; return BT::NodeStatus::RUNNING; } else { @@ -68,10 +45,10 @@ BT::NodeStatus Execution::onRunning(){ // callback to execute if the action was aborted by another node void Execution::onHalted(){} -void Execution::init(Moveit_mediator* mediator_reference ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) { +void Execution::init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) { mr_reference_= mr_reference; publisher_reference_ = publisher_reference; ets_ = ets; it_ = ets_.solution.sub_trajectory.begin(); - mediator_reference_ = mediator_reference_; + executions_ = executions; } \ No newline at end of file diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp index 594f55c93e6f0f4909902c6586bb72586d8781aa..032dbf08ec3f56a4a1cea714a36f7794e38a23ef 100644 --- a/src/impl/moveit_mediator.cpp +++ b/src/impl/moveit_mediator.cpp @@ -879,6 +879,8 @@ void Moveit_mediator::task_planner(){ auto tree = factory.createTreeFromText(ss.str()); auto node_it = tree.nodes.begin(); + + for(int i =0; i < robots_.size();i++){ try{ for (auto& p_obj: task_but_different.at(robots_[i]->name())){ @@ -891,7 +893,7 @@ void Moveit_mediator::task_planner(){ if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("init Condition");condition->init(std::get<0>(p_obj), std::get<1>(p_obj), psi_.get());++node_it;} for(int j = 0; j < std::get<2>(p_obj).size(); j++){ auto mr = dynamic_cast<Moveit_robot*>(robots_[i]); - if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("init Action");execution->init(this, planning_scene_diff_publisher_.get(), mr, std::get<2>(p_obj)[j]);++node_it;} + if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("init Action");execution->init(&executions_, planning_scene_diff_publisher_.get(), mr, std::get<2>(p_obj)[j]);++node_it;} } ss << "</SequenceStar>\n"; } @@ -906,13 +908,19 @@ void Moveit_mediator::task_planner(){ while( status == BT::NodeStatus::RUNNING){ status = tree.tickRoot(); - for(int i = 0; i < executions_.size(); i++){ - if(executions_[i]->joinable()) executions_[i]->join(); + std::vector<std::thread> th; + for(auto& exec : executions_){ + for (int i = 0; i < robots_.size(); i++){ + if (exec.first == robots_[i]->name()){ + auto mr = dynamic_cast<Moveit_robot*>(robots_[i]); + th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second)); + } + } + } + + for(auto& t : th){ + if(t.joinable()) t.join(); } - // IMPORTANT: you must always add some sleep if you call tickRoot() - // in a loop, to avoid using 100% of your CPU (busy loop). - // The method Tree::sleep() is recommended, because it can be - // interrupted by an internal event inside the tree. } @@ -1013,15 +1021,11 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla } -/* + void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt){ mr.mgi()->execute(rt); } -*/ -void Moveit_mediator::parallel_exec(){ - //ROS_INFO("UwU"); - // mr.mgi()->execute(rt); -} + @@ -1429,8 +1433,6 @@ Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> object , job_reader_(std::make_unique<Job_reader>(nh_)) , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) { - executions_.resize(2); - if (planning_scene_monitor_->getPlanningScene()){ planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, "planning_scene");