diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp
index aa2434ec7b7a6ee32fab8e6a3eb19ed7c8577a36..040baa46a063db388d907c8fc65b286c6b1cb4f4 100644
--- a/src/bt/execution.cpp
+++ b/src/bt/execution.cpp
@@ -23,14 +23,9 @@ BT::NodeStatus Execution::tick() {
BT::NodeStatus Execution::onStart(){
if (it_ != ets_.solution.sub_trajectory.end()){
std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
- for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_names) ROS_INFO("%s", e.c_str());
- for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_values) {
- for (auto en: e.enabled){
- ROS_INFO("%f", en);
- }
- }
executions_->insert_or_assign(mr_reference_->name(), pair);
it_++;
+ ROS_INFO("oihinin");
return BT::NodeStatus::RUNNING;
} else {
return BT::NodeStatus::SUCCESS;
@@ -42,14 +37,10 @@ BT::NodeStatus Execution::onStart(){
BT::NodeStatus Execution::onRunning(){
if (it_ != ets_.solution.sub_trajectory.end()){
std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
- for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_names) ROS_INFO("%s", e.c_str());
- for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_values) {
- for (auto en: e.enabled){
- ROS_INFO("%f", en);
- }
- }
executions_->insert_or_assign(mr_reference_->name(), pair);
it_++;
+ ROS_INFO("oihinin");
+
return BT::NodeStatus::RUNNING;
} else {
return BT::NodeStatus::SUCCESS;
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index 8e466ff91de188fa7048a3600452e1c27cd47941..d9ce639e755bef284a2496017c0d396a80957178 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -15,6 +15,7 @@
#include "bt/position_condition.h"
#include "bt/parallel_robot.h"
#include <actionlib_msgs/GoalStatusArray.h>
+#include <chrono>
@@ -119,448 +120,6 @@ void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>,
void Moveit_mediator::setup_task(){
ROS_INFO("=> write Task Constructor Objects");
for(auto& a: acm_) a.second.resize(acm_.size(), 0);
- /*
-
- bool coop = false;
-
- for(int i = 0; i < objects_.size(); i++){
- if (i+1 < objects_.size()){
- for (long unsigned int j = objects_[i].size()-1; j > 0; j--){
- if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) {
- objects_[i+1].pop_back();
- coop= true;
- }
- }
- }
- }
-
- if(coop){
- moveit_msgs::CollisionObject bottle;
- bottle.id = "bottle";
- bottle.header.frame_id = "world";
- bottle.header.stamp = ros::Time::now();
- bottle.primitives.resize(1);
- bottle.primitives[0].type = bottle.primitives[0].BOX;
- bottle.primitives[0].dimensions.resize(3);
- bottle.primitives[0].dimensions[0] = box_size.getX();
- bottle.primitives[0].dimensions[1] = box_size.getY();
- bottle.primitives[0].dimensions[2] = box_size.getZ();
-
- bottle.primitive_poses.resize(1);
- bottle.primitive_poses[0].position.x = -0.3f;
- bottle.primitive_poses[0].position.y = -0.6f;
- bottle.primitive_poses[0].position.z = 0.9355f;
- bottle.primitive_poses[0].orientation.x = 0;
- bottle.primitive_poses[0].orientation.y = 0;
- bottle.primitive_poses[0].orientation.z = 0;
- bottle.primitive_poses[0].orientation.w = 1;
- bottle.operation = bottle.ADD;
-
- psi_->applyCollisionObject(bottle);
-
-
- for( int i = 0; i < objects_.size();i++){
- for(int j =0; j < objects_[i].size();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)){
- task.execute(*task.solutions().front());
- }
-
- }
- }
- } else {
- acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle0", std::vector<uint8_t>()));
- acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle1", std::vector<uint8_t>()));
-
- moveit_msgs::CollisionObject bottle1;
- bottle1.id = "bottle0";
- bottle1.header.frame_id = "world";
- bottle1.header.stamp = ros::Time::now();
- bottle1.primitives.resize(1);
- bottle1.primitives[0].type = bottle1.primitives[0].BOX;
- bottle1.primitives[0].dimensions.resize(3);
- bottle1.primitives[0].dimensions[0] = box_size.getX();
- bottle1.primitives[0].dimensions[1] = box_size.getY();
- bottle1.primitives[0].dimensions[2] = box_size.getZ();
-
- bottle1.primitive_poses.resize(1);
- bottle1.primitive_poses[0].position.x = objects_[0][0].getOrigin().getX();
- bottle1.primitive_poses[0].position.y = objects_[0][0].getOrigin().getY();
- bottle1.primitive_poses[0].position.z = 0.9355f;
- bottle1.primitive_poses[0].orientation.x = 0;
- bottle1.primitive_poses[0].orientation.y = 0;
- bottle1.primitive_poses[0].orientation.z = 0;
- bottle1.primitive_poses[0].orientation.w = 1;
- bottle1.operation = bottle1.ADD;
-
- moveit_msgs::CollisionObject bottle2;
- bottle2.id = "bottle1";
- bottle2.header.frame_id = "world";
- bottle2.header.stamp = ros::Time::now();
- bottle2.primitives.resize(1);
- bottle2.primitives[0].type = bottle2.primitives[0].BOX;
- bottle2.primitives[0].dimensions.resize(3);
- bottle2.primitives[0].dimensions[0] = box_size.getX();
- bottle2.primitives[0].dimensions[1] = box_size.getY();
- bottle2.primitives[0].dimensions[2] = box_size.getZ();
-
- bottle2.primitive_poses.resize(1);
- bottle2.primitive_poses[0].position.x = objects_[1][0].getOrigin().getX();
- bottle2.primitive_poses[0].position.y = objects_[1][0].getOrigin().getY();
- bottle2.primitive_poses[0].position.z = 0.9355f;
- bottle2.primitive_poses[0].orientation.x = 0;
- bottle2.primitive_poses[0].orientation.y = 0;
- bottle2.primitive_poses[0].orientation.z = 0;
- bottle2.primitive_poses[0].orientation.w = 1;
- bottle2.operation = bottle2.ADD;
-
- psi_->applyCollisionObject(bottle1);
- psi_->applyCollisionObject(bottle2);
-
- std::vector<std::thread> ths;
- std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> tasks;
-
- for (int i = 0 ; i < objects_.size(); i++){
- std::stringstream ss;
- ss << "bottle" << std::to_string(i);
- Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
- std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
- for (int j = 1; j < objects_[i].size(); j++){
- moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(ss.str()), objects_[i][j]);
- if(mgt.plan(1)) {
- moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
- mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
- tasks_per_robot.push(e);
-
- moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
- temp.operation = temp.MOVE;
- temp.pose.position.x = objects_[i][j].getOrigin().getX();
- temp.pose.position.y = objects_[i][j].getOrigin().getY();
- psi_->applyCollisionObject(temp);
- }
- }
-
- moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
- temp.operation = temp.MOVE;
- temp.pose.position.x = objects_[i][0].getOrigin().getX();
- temp.pose.position.y = objects_[i][0].getOrigin().getY();
- psi_->applyCollisionObject(temp);
- tasks.push_back(tasks_per_robot);
- //ths.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
- }
-
- for(auto& a: acm_) a.second.resize(acm_.size(), 0);
-
- while (!tasks[0].empty() && !tasks[1].empty()){
- 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* t1 = nullptr;
- moveit_msgs::RobotTrajectory* t2 = nullptr;
- moveit_msgs::PlanningScene* ps1 = nullptr;
- moveit_msgs::PlanningScene* ps2 = nullptr;
- moveit_msgs::PlanningScene ps_m;
- std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_m;
-
- //for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str());
-
- if (i < tasks[0].front().solution.sub_trajectory.size()){
- t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
- ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff;
- }
-
- if (i < tasks[1].front().solution.sub_trajectory.size()){
- t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
- ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
- }
-
- std::vector<std::thread> th;
-
- if (t1){
- Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
- th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
-
- // First find ID from panda to start with
- std::regex rx_panda("panda_arm([0-9]+)");
- std::smatch match;
- std::regex_match(mr->name(), match, rx_panda);
-
- // build panda link regex
- std::stringstream ss;
- ss << "panda_" << match[1] << "_.*";
- std::regex rx_panda_links(ss.str());
- std::regex rx_box("bottle.*");
- std::regex rx_table("table.*");
-
- // Iterate task collsion matrix
- for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
- if( mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
- for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
-
- if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
- for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
-
- if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box)){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
- for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
-
- if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], j);
- for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
- }
- merge_ps(ps_m, ps1, mr);
- }
-
- if (t2){
- Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
- th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));
-
- std::regex rx_panda("panda_arm([0-9]+)");
- std::smatch match;
- std::regex_match(mr->name(), match, rx_panda);
-
- // build panda link regex
- std::stringstream ss;
- ss << "panda_" << match[1] << "_.*";
- std::regex rx_panda_links(ss.str());
- std::regex rx_box("bottle.*");
- std::regex rx_table("table.*");
-
- // Iterate task collsion matrix
- for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
- if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
- for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
-
- if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
- for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
-
- if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
- for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
-
- if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table)){
- In case an entry matches an robot spezific link
- ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j], j);
- for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- For that specific entry, loop over values
- int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
- ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
-
- if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
- }
- }
- }
- }
- merge_ps(ps_m, ps2, mr);
- }
-
-
- for(int i = 0; i < th.size(); i++){
- th[i].join();
- }
- merge_acm(ps_m);
- planning_scene_diff_publisher_->publish(ps_m);
-
- }
- tasks[0].pop();
- tasks[1].pop();
- }
- }
- */
}
@@ -570,22 +129,14 @@ void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScen
std::smatch match;
std::regex_match(mr->name(), match, rx_panda);
- ROS_INFO("manipulate_acm");
- for (auto& e : ps.allowed_collision_matrix.entry_names) ROS_INFO("%s", e.c_str());
- for (auto& e : ps.allowed_collision_matrix.entry_values) {
- for (auto en: e.enabled){
- ROS_INFO("%f", en);
- }
- }
-
-
// build panda link regex
std::stringstream ss;
ss << "panda_" << match[1] << "_.*";
std::regex rx_panda_links(ss.str());
- std::regex rx_box("box.*");
+ std::regex rx_box("box_.*");
std::regex rx_table("table.*");
+
// Iterate task collsion matrix
for(int j = 0; j < ps.allowed_collision_matrix.entry_names.size(); j++ ){
if( mr->map().at("base") == ps.allowed_collision_matrix.entry_names[j]){
@@ -598,22 +149,26 @@ void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScen
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
}
@@ -627,22 +182,26 @@ void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScen
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
}
@@ -656,22 +215,26 @@ void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScen
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
}
@@ -685,26 +248,38 @@ void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScen
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
}
}
+
+ ROS_INFO("after run");
+ for(auto& entry : acm_){
+ ROS_INFO("%s", entry.first.c_str());
+ for (auto& enable : entry.second) std::cout << +enable << " ";
+ std::cout<< "\n";
+ }
+
}
@@ -785,6 +360,7 @@ void Moveit_mediator::task_planner(){
for (int k = 1; k < temp.second.jobs_.size(); k++){
moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
if(mgt.plan(1)) {
+
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
bt_list.push_back(e);
@@ -876,8 +452,6 @@ 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())){
@@ -904,38 +478,56 @@ void Moveit_mediator::task_planner(){
BT::NodeStatus status = BT::NodeStatus::RUNNING;
while( status == BT::NodeStatus::RUNNING){
status = tree.tickRoot();
+
+ // std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
std::vector<std::thread> th;
moveit_msgs::PlanningScene ps_m;
+ bool is_executing = !(executions_.empty());
+
+
+ for (int i = 0; i < robots_.size(); i++){
+ try{
+ auto temp = executions_.at(robots_[i]->name());
+ auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
+ th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), temp.first));
+ //mr->mgi()->execute(temp.first);
+ // ROS_INFO("acm in msg");
+ // for(int k = 0; k < temp.second.allowed_collision_matrix.entry_names.size(); k++){
+ // ROS_INFO("%s", temp.second.allowed_collision_matrix.entry_names[k].c_str());
+ // for (auto& enable : temp.second.allowed_collision_matrix.entry_values[k].enabled) std::cout << +enable << " ";
+ // std::cout<< "\n";
+ // }
+
+ manipulate_acm(mr, temp.second);
+ merge_ps(ps_m, temp.second, mr);
+ executions_.erase(robots_[i]->name());
+ } catch (std::out_of_range& oor){}
- 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.first));
- //mr->mgi()->execute(exec.second.first);
- //manipulate_acm(mr, exec.second.second);
- //merge_ps(ps_m, exec.second.second, mr);
- }
- }
}
for(auto& t : th){
t.join();
}
-
- 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]);
- manipulate_acm(mr, exec.second.second);
- merge_ps(ps_m, exec.second.second, mr);
- }
- }
+ if (is_executing){
+ merge_acm(ps_m);
+ planning_scene_diff_publisher_->publish(ps_m);
}
+
+
+ // 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]);
+ // manipulate_acm(mr, exec.second.second);
+ // merge_ps(ps_m, exec.second.second, mr);
- merge_acm(ps_m);
- planning_scene_diff_publisher_->publish(ps_m);
+ // }
+ // }
+ // }
+
+
}
@@ -982,14 +574,15 @@ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
i++;
}
-
- ROS_INFO("Merge ACM");
- for (auto& e : acmt.entry_names) ROS_INFO("%s", e.c_str());
- for (auto& e : acmt.entry_values) {
- for (auto en: e.enabled){
- ROS_INFO("%f", en);
- }
- }
+ // ROS_INFO("merge_acm");
+ // for(int k = 0; k < acmt.entry_names.size(); k++){
+ // ROS_INFO("%s", acmt.entry_names[k].c_str());
+ // for (auto& enable : acmt.entry_values[k].enabled){
+ // std::cout << +enable << " ";
+ // }
+ // std::cout<< "\n";
+ // }
+
ps_m.allowed_collision_matrix = acmt;
}
@@ -1035,8 +628,7 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla
out.link_scale.push_back(in.link_scale[i]);
}
}
- }
-
+ }
}