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

...

parent 703fb181
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/bt/execution.cpp
+ 3
− 12
View file @ 525d0639
......@@ -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;
......
src/impl/moveit_mediator.cpp
+ 93
− 501
View file @ 525d0639
......@@ -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())){
......@@ -905,40 +479,58 @@ void Moveit_mediator::task_planner(){
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(auto& exec : executions_){
for (int i = 0; i < robots_.size(); i++){
if (exec.first == robots_[i]->name()){
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), exec.second.first));
//mr->mgi()->execute(exec.second.first);
//manipulate_acm(mr, exec.second.second);
//merge_ps(ps_m, exec.second.second, mr);
}
}
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& 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);
// }
// }
// }
}
// clean up
for (int i = 0; i < cd.size(); i ++){
std::stringstream ss;
......@@ -982,14 +574,15 @@ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
i++;
}
// 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";
// }
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);
}
}
ps_m.allowed_collision_matrix = acmt;
}
......@@ -1036,7 +629,6 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla
}
}
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment