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

container push

parent a6660e37
No related branches found
No related tags found
No related merge requests found
......@@ -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 {}; }
......
......@@ -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;};
......
......@@ -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
......@@ -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");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment