Select Git revision
SampleConstraintPlanner.cpp
Forked from
CeTI / ROS / ROS Packages / panda_simulation
Source project has a limited visibility.
moveit_mediator.cpp 59.99 KiB
#include "impl/moveit_mediator.h"
#include <thread>
#include <mutex>
#include <algorithm>
#include <gb_grasp/MapGenerator.h>
#include <regex>
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/tree_node.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
#include "bt/planning_scene.h"
#include "bt/trajetory.h"
#include "bt/execution.h"
#include "bt/position_condition.h"
#include "bt/parallel_robot.h"
#include <actionlib_msgs/GoalStatusArray.h>
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::connect_robots(Abstract_robot* robot) {
robots_.push_back(robot);
ROS_INFO("%s connected...", robot->name().c_str());
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->map().at("base").c_str(), std::vector<uint8_t>()));
for(auto name: mr->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
}
void Moveit_mediator::publish_tables(){
ros::WallDuration sleep_time(1.0);
for(long unsigned int i = 0; i < robots_.size();i++){
Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
/*
moveit_msgs::CollisionObject table;
table.id = ceti->map().at("base").c_str();
table.header.frame_id = "world";
table.header.stamp = ros::Time::now();
table.primitives.resize(1);
table.primitives[0].type = table.primitives[0].BOX;
table.primitives[0].dimensions.resize(3);
table.primitives[0].dimensions[0] = ceti->size().getX();
table.primitives[0].dimensions[1] = ceti->size().getY();
table.primitives[0].dimensions[2] = ceti->tf().getOrigin().getZ() * 2;
table.primitive_poses.resize(1);
table.primitive_poses[0].position.x = ceti->tf().getOrigin().getX();
table.primitive_poses[0].position.y = ceti->tf().getOrigin().getY();
table.primitive_poses[0].position.z = ceti->tf().getOrigin().getZ();
table.primitive_poses[0].orientation.x = ceti->tf().getRotation().getX();
table.primitive_poses[0].orientation.y = ceti->tf().getRotation().getY();
table.primitive_poses[0].orientation.z = ceti->tf().getRotation().getZ();
table.primitive_poses[0].orientation.w = ceti->tf().getRotation().getW();
table.operation = table.ADD;
*/
//psi_->applyCollisionObject(table);
for (Abstract_robot_element* are : ceti->observers()) {
Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
psi_->applyCollisionObject(*wmd->markers());
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->markers()->id.c_str(), std::vector<uint8_t>()));
sleep_time.sleep();
}
}
}
bool Moveit_mediator::check_collision(const int& robot){
auto env = ps_->getCollisionEnvNonConst();
Collision_helper* ch = static_cast<Collision_helper*>(env.get());
return ch->collision_detector("object_A", "object_B"); //
}
void Moveit_mediator::mediate(){
publish_tables();
task_planner();
setup_task();
ros::waitForShutdown();
};
void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
std::bitset<3> result = robots_[robot]->observer_mask() & wing;
Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
for (std::size_t i = 0; i < result.size(); i++){
if (result[i]){
ceti->register_observers(wings_[robot][i]);
}
}
}
void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
for (int i =0; i < wbp.size(); i++){
std::vector<Abstract_robot_element*> v;
for (int j =0; j < wbp[i].first.size(); j++){
Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
v.push_back(are);
}
wings_.push_back(v);
}
};
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();
}
}
*/
}
void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps){
// 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("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]){
//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 < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
// For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
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];
}
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];
}
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];
}
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];
}
}
}
if(std::regex_match(ps.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", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
//For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
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];
}
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];
}
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];
}
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];
}
}
}
if(std::regex_match(ps.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", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
//For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
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];
}
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];
}
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];
}
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];
}
}
}
if(std::regex_match(ps.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", ps.allowed_collision_matrix.entry_names[j], j);
for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
//For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
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];
}
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];
}
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];
}
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];
}
}
}
}
}
void Moveit_mediator::task_planner(){
/* There are 2 ways to interprete a Task
1. A box position in acm is also the first entry in task,
2. A box position in acm is not the first entry in task, in that case we can might try for each position
*/
auto jq = job_reader_->robot_data();
auto cd = cuboid_reader_->cuboid_bin();
//std::vector<std::string> objs = {"bottle1", "bottle2"};
for (int i = 0; i < cd.size(); i ++){
std::stringstream ss;
ss << "box_" << i;
moveit_msgs::CollisionObject item;
item.id = ss.str();
item.header.frame_id = "world";
item.header.stamp = ros::Time::now();
item.primitives.resize(1);
item.primitives[0].type = item.primitives[0].BOX;
item.primitives[0].dimensions.resize(3);
item.primitives[0].dimensions[0] = cd[i].x_depth;
item.primitives[0].dimensions[1] = cd[i].y_width;
item.primitives[0].dimensions[2] = cd[i].z_heigth;
item.primitive_poses.resize(1);
item.primitive_poses[0].position.x = cd[i].Pose.position.x;
item.primitive_poses[0].position.y = cd[i].Pose.position.y;
item.primitive_poses[0].position.z = cd[i].Pose.position.z;
item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
item.operation = item.ADD;
psi_->applyCollisionObject(item);
acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
// Could also safe id's somewhere
}
// Setup shared ACM
for(auto& a: acm_) a.second.resize(acm_.size(), 0);
std::regex item("box_([0-9]+)");
std::smatch match;
// ----------------------------------
// GROOT Strategie => erst FROM TEXT
// ----------------------------------
BT::BehaviorTreeFactory factory;
factory.registerNodeType<Planning_scene>("PS");
factory.registerNodeType<Trajetory>("Trajetory");
factory.registerNodeType<Execution>("Execution");
factory.registerNodeType<Position_condition>("Position_condition");
factory.registerNodeType<Parallel_robot>("Parallel_robot");
std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
while(!jq.empty()){
for(auto& s_co : psi_->getObjects()){
if(!std::regex_match(s_co.first, match, item)) continue;
std::pair<std::string, job_data> temp = jq.front();
ROS_INFO("1. job entry %f %f %f", temp.second.jobs_.front().getOrigin().getX(), temp.second.jobs_.front().getOrigin().getY(), temp.second.jobs_.front().getOrigin().getZ());
ROS_INFO("object position %f %f %f", s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z);
if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z )) == 0) {
std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list;
Moveit_robot* mr;
for(auto* r: robots_) if (r->name() == temp.first) mr = dynamic_cast<Moveit_robot*>(r);
// loop jobs
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);
moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
temp_co.operation = temp_co.MOVE;
temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
psi_->applyCollisionObject(temp_co);
}
}
/*
if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("om");condition->init(s_co.first, temp.second.jobs_.front().getOrigin(), psi_.get()); ++node_it;}
for(auto& ets : bt_list)
if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("tr");execution->init(planning_scene_diff_publisher_.get(), mr->mgi().get(), ets); ++node_it;}
*/
for (int i = 0; i < cd.size(); i ++){
std::stringstream ss;
ss << "box_" << i;
moveit_msgs::CollisionObject item;
item.id = ss.str();
item.header.frame_id = "world";
item.header.stamp = ros::Time::now();
item.primitives.resize(1);
item.primitives[0].type = item.primitives[0].BOX;
item.primitives[0].dimensions.resize(3);
item.primitives[0].dimensions[0] = cd[i].x_depth;
item.primitives[0].dimensions[1] = cd[i].y_width;
item.primitives[0].dimensions[2] = cd[i].z_heigth;
item.pose.position.x = cd[i].Pose.position.x;
item.pose.position.y = cd[i].Pose.position.y;
item.pose.position.z = cd[i].Pose.position.z;
item.pose.orientation.x = cd[i].Pose.orientation.x;
item.pose.orientation.y = cd[i].Pose.orientation.y;
item.pose.orientation.z = cd[i].Pose.orientation.z;
item.pose.orientation.w = cd[i].Pose.orientation.w;
item.operation = item.MOVE;
psi_->applyCollisionObject(item);
// Could also safe id's somewhere
}
try{
task_but_different.at(mr->name()).push_back(std::tuple<std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list));
} catch(std::out_of_range &oor) {
std::tuple<const std::string&, tf2::Vector3&, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>&> tuple(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list);
task_but_different.insert(std::pair<std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>>(mr->name(),{tuple}));
}
jq.pop_front();
} else {jq.push_back(temp);}
}
}
std::stringstream ss;
ss << "<root main_tree_to_execute = \"MainTree\">\n";
ss << "<BehaviorTree ID=\"MainTree\">\n";
ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< robots_.size() << "\" failure_threshold=\""<< robots_.size() << "\">\n";
for(int i =0; i < robots_.size();i++){
try{
ss << "<Control ID=\"Parallel_robot\" name=\""<< robots_[i]->name() << "\" success_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\" failure_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\">\n";
for (auto& p_obj: task_but_different.at(robots_[i]->name())){
ss << "<SequenceStar name=\"root_sequence\">\n";
ss << "<Condition ID=\"Position_condition\" name=\"Position_condition\"/>\n";
for(int j = 0; j < std::get<2>(p_obj).size(); j++){
ss << "<Action ID=\"Execution\" name=\"Execution\"/>\n";
}
ss << "</SequenceStar>\n";
}
ss << "</Control>\n";
} catch(std::out_of_range& oor){}
}
ss << "</Control>\n";
ss << "</BehaviorTree>\n";
ss << "</root>\n";
std::cout << ss.str();
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())){
while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it;
if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action");
if(node_it->get()->type() == BT::NodeType::CONDITION) ROS_INFO("Condition");
if(node_it->get()->type() == BT::NodeType::CONTROL) ROS_INFO("Control");
if(node_it->get()->type() == BT::NodeType::DECORATOR) ROS_INFO("Decorator");
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(&executions_, planning_scene_diff_publisher_.get(), mr, std::get<2>(p_obj)[j]);++node_it;}
}
ss << "</SequenceStar>\n";
}
ss << "</Control>\n";
} catch(std::out_of_range& oor){}
}
BT::PublisherZMQ zmq(tree);
ros::Duration sleep(1);
BT::NodeStatus status = BT::NodeStatus::RUNNING;
while( status == BT::NodeStatus::RUNNING){
status = tree.tickRoot();
std::vector<std::thread> th;
moveit_msgs::PlanningScene 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]);
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);
}
}
}
merge_acm(ps_m);
planning_scene_diff_publisher_->publish(ps_m);
}
// clean up
for (int i = 0; i < cd.size(); i ++){
std::stringstream ss;
ss << "box_" << i;
moveit_msgs::CollisionObject item;
item.id = ss.str();
item.header.frame_id = "world";
item.header.stamp = ros::Time::now();
item.primitives.resize(1);
item.primitives[0].type = item.primitives[0].BOX;
item.primitives[0].dimensions.resize(3);
item.primitives[0].dimensions[0] = cd[i].x_depth;
item.primitives[0].dimensions[1] = cd[i].y_width;
item.primitives[0].dimensions[2] = cd[i].z_heigth;
item.pose.position.x = cd[i].Pose.position.x;
item.pose.position.y = cd[i].Pose.position.y;
item.pose.position.z = cd[i].Pose.position.z;
item.pose.orientation.x = cd[i].Pose.orientation.x;
item.pose.orientation.y = cd[i].Pose.orientation.y;
item.pose.orientation.z = cd[i].Pose.orientation.z;
item.pose.orientation.w = cd[i].Pose.orientation.w;
item.operation = item.MOVE;
psi_->applyCollisionObject(item);
// Could also safe id's somewhere
}
}
void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
acmt.entry_values.resize(acm_.size());
int i = 0;
for (auto& a : acm_){
acmt.entry_names.push_back(a.first);
acmt.entry_values[i].enabled = a.second;
i++;
}
ps_m.allowed_collision_matrix = acmt;
}
void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status){
if (!status->status_list.empty()){
ROS_INFO("%i", status->status_list.size());
}
}
void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr){
// get full mr link list
std::vector<std::string> links;
for (auto link : mr->mgi()->getLinkNames())links.push_back(link);
links.push_back(mr->map()["left_finger"]);
links.push_back(mr->map()["right_finger"]);
links.push_back(mr->map()["hand_link"]);
links.push_back(mr->map()["base"]);
for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
if (in.robot_state.is_diff) out.robot_state.is_diff = in.robot_state.is_diff;
if (in.is_diff) out.is_diff = in.is_diff;
out.robot_state.joint_state.header = in.robot_state.joint_state.header;
out.robot_model_name = "panda";
for (auto link : links) {
for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
if(link.c_str() == in.robot_state.joint_state.name[i].c_str()){
out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
ROS_INFO("check");
}
}
for(int i = 0; i < in.link_padding.size(); i++){
if(link.c_str() == in.link_padding[i].link_name.c_str()){
out.link_padding.push_back(in.link_padding[i]);
}
}
for(int i = 0; i < in.link_scale.size(); i++){
if(link.c_str() == in.link_scale[i].link_name.c_str()){
out.link_scale.push_back(in.link_scale[i]);
}
}
}
}
void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt){
mr.mgi()->execute(rt);
}
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";
for (Abstract_robot* ar : robots_){
std::string str;
if(ar->check_single_object_collision(t, str)) support_surface1 = str;
if(ar->check_single_object_collision(target, str)) support_surface2= str;
}
ROS_INFO("ss1 %s", support_surface1.c_str());
ROS_INFO("ss2 %s", support_surface2.c_str());
const std::string object = source.id;
moveit::task_constructor::Task task_;
std::string name = "Pick&Place";
task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel());
// Set task properties
task_.setProperty("group", mr->name());
task_.setProperty("eef", mr->map()["eef_name"]);
task_.setProperty("hand", mr->map()["hand_group_name"]);
task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
task_.setProperty("ik_frame", mr->map()["hand_frame"]);
moveit::task_constructor::Stage* current_state_ptr = nullptr;
{
auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
comment = "object with id '" + object + "' is already attached and cannot be picked";
return false;
}
return true;
});
current_state_ptr = applicability_filter.get();
task_.add(std::move(applicability_filter));
}
{ // Open Hand
auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("open");
task_.add(std::move(stage));
}
{ // Move-to pre-grasp
auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
"move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_} });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task_.add(std::move(stage));
}
moveit::task_constructor::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
{ // Approach Obj
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_);
stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", mr->map()["hand_frame"]);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.07, 0.2);
// Set hand forward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = mr->map()["hand_frame"];
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
{
// Sample grasp pose
auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose("open");
stage->setObject(object);
stage->setAngleDelta(M_PI / 12);
stage->setMonitoredStage(current_state_ptr); // Hook into current state
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(8);
wrapper->setMinSolutionDistance(1.0);
wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
grasp->insert(std::move(wrapper));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
true);
grasp->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("close");
grasp->insert(std::move(stage));
}
{ // Attach obj
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, mr->map()["hand_frame"]);
attach_object_stage = stage.get();
grasp->insert(std::move(stage));
}
{ // Allow Collision obj table
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
stage->allowCollisions({ object }, support_surface1, true);
grasp->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.1, 0.2);
stage->setIKFrame(mr->map()["hand_frame"]);
stage->properties().set("marker_ns", "lift_object");
// Set upward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
{ // forbid collision
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ object }, support_surface1, false);
grasp->insert(std::move(stage));
}
// Add grasp container to task
task_.add(std::move(grasp));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
"move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
task_.add(std::move(stage));
}
{
auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)");
stage->allowCollisions( {object} , support_surface2, true);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_);
stage->properties().set("marker_ns", "lower_object");
stage->properties().set("link", mr->map()["hand_frame"]);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.03, .13);
// Set downward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = "world";
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
{
// Generate Place Pose
auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
stage->properties().set("marker_ns", "place_pose");
stage->setObject(object);
// Set target pose
geometry_msgs::PoseStamped p;
p.header.frame_id = "world";
p.pose.position.x = target.getOrigin().getX();
p.pose.position.y = target.getOrigin().getY();
p.pose.position.z = target.getOrigin().getZ();
p.pose.orientation.x = target.getRotation().getX();
p.pose.orientation.y = target.getRotation().getY();
p.pose.orientation.z = target.getRotation().getZ();
p.pose.orientation.w = target.getRotation().getW();
stage->setPose(p);
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
// Compute IK
Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
Eigen::Translation3d trans(0.1f, 0, 0);
Eigen::Isometry3d ik = eigen * trans;
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
wrapper->setMaxIKSolutions(2);
wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
place->insert(std::move(wrapper));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("open");
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
false);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
stage->detachObject(object, mr->map()["hand_frame"]);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setMinMaxDistance(.1, .2);
stage->setIKFrame(mr->map()["hand_frame"]);
stage->properties().set("marker_ns", "retreat");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = mr->map()["hand_frame"];
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
stage->setGoal("close");
place->insert(std::move(stage));
}
// Add place container to task
task_.add(std::move(place));
}
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_);
stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
stage->setGoal("ready");
stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
task_.add(std::move(stage));
}
return task_;
}
Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
: Abstract_mediator(objects, pub), nh_(nh)
, sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
, cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
, psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
, planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
, job_reader_(std::make_unique<Job_reader>(nh_))
, cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
if (planning_scene_monitor_->getPlanningScene()){
planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
"planning_scene");
planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
} else {
ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
return;
}
robot_model_loader::RobotModelLoaderPtr robot_model_loader;
robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
robot_model_ = robot_model_loader->getModel();
ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
// planner
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
// cartesian
cartesian_planner_->setMaxVelocityScaling(1.0);
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
};