Select Git revision
moveit_mediator.cpp 44.97 KiB
#include "impl/moveit_mediator.h"
#include <thread>
#include <mutex>
#include <algorithm>
#include <gb_grasp/MapGenerator.h>
#include <regex>
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();
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");
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 (int i = 0; i < ths.size(); i++) ths[i].join();
std::vector<std::vector<std::vector<moveit_msgs::RobotTrajectory>>> rt_r_t;
for (int i =0; i < tasks.size(); i++){
std::vector<std::vector<moveit_msgs::RobotTrajectory>> rt_r;
while (!tasks[i].empty()){
std::vector<moveit_msgs::RobotTrajectory> rt;
for(auto sub_t : tasks[i].front().solution.sub_trajectory){
rt.push_back(sub_t.trajectory);
}
tasks[i].pop();
rt_r.push_back(rt);
}
rt_r_t.push_back(rt_r);
}
*/
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;
ROS_INFO("[Robot 1] acm");
ROS_INFO("default entry names");
for (auto den : ps1->allowed_collision_matrix.default_entry_names){
ROS_INFO("%s", den.c_str());
}
ROS_INFO("default entry values");
for (auto den : ps1->allowed_collision_matrix.default_entry_values){
ROS_INFO("%i", den);
}
ROS_INFO("entry names");
for (auto den : ps1->allowed_collision_matrix.entry_names){
ROS_INFO("%s", den.c_str());
ROS_INFO("count %i", ps1->allowed_collision_matrix.entry_names.size());
}
ROS_INFO("entry values");
for (auto den : ps1->allowed_collision_matrix.entry_values){
for (auto ene : den.enabled) ROS_INFO("%i", ene);
ROS_INFO("count %i", den.enabled.size());
}
ROS_INFO("[Robot 1] fft");
for (auto ft : ps1->fixed_frame_transforms){
ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq);
}
ROS_INFO("[Robot 1] is_diff");
ROS_INFO("is_diff: %i", ps1->is_diff);
ROS_INFO("[Robot 1] link padding");
for (auto lp : ps1->link_padding){
ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding);
}
ROS_INFO("[Robot 1] link scale");
for (auto ls : ps1->link_scale){
ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale);
}
ROS_INFO("[Robot 1] name");
ROS_INFO("name: %s", ps1->name.c_str());
ROS_INFO("[Robot 1] object color");
for (auto oc : ps1->object_colors){
ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str());
}
ROS_INFO("[Robot 1] rmn");
ROS_INFO("rmn %s", ps1->robot_model_name.c_str());
ROS_INFO("[Robot 1] rs");
for (auto aco : ps1->robot_state.attached_collision_objects){
ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq);
for (auto jn : aco.detach_posture.joint_names){
ROS_INFO("aco dp joint name: %s", jn.c_str());
}
ROS_INFO("aco dp Points");
for (auto p : aco.detach_posture.points){
for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa);
for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa);
ROS_INFO("aco dp points time from start");
for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa);
for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa);
}
ROS_INFO("aco link names %s", aco.link_name.c_str());
ROS_INFO("aco collis obj id %s", aco.object.id.c_str());
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str());
ROS_INFO("aco collis obj weight %f", aco.weight);
}
for (auto d : ps1->robot_state.joint_state.position) ROS_INFO("js position %f ", d);
for (auto d : ps1->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
for (auto d : ps1->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d);
for (auto d : ps1->robot_state.joint_state.effort) ROS_INFO(" js e %f", d);
ROS_INFO ("%s, %i", ps1->robot_state.joint_state.header.frame_id.c_str(), ps1->robot_state.joint_state.header.seq);
for (auto p : ps1->world.collision_objects) ROS_INFO("world co id %s", p.id);
ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps1->world.octomap.origin.position.x, ps1->world.octomap.origin.position.y, ps1->world.octomap.origin.position.z, ps1->world.octomap.origin.orientation.x, ps1->world.octomap.origin.orientation.y, ps1->world.octomap.origin.orientation.z, ps1->world.octomap.origin.orientation.w);
ROS_INFO("world om header time, id %s, seq %i", ps1->world.octomap.header.frame_id.c_str(), ps1->world.octomap.header.seq);
ROS_INFO("world om Data");
}
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;
ROS_INFO("[Robot 2] acm");
ROS_INFO("default entry names");
for (auto den : ps2->allowed_collision_matrix.default_entry_names){
ROS_INFO("%s", den.c_str());
}
ROS_INFO("default entry values");
for (auto den : ps2->allowed_collision_matrix.default_entry_values){
ROS_INFO("%i", den);
} ROS_INFO("entry names");
for (auto den : ps2->allowed_collision_matrix.entry_names){
ROS_INFO("%s", den.c_str());
ROS_INFO("count %i", ps2->allowed_collision_matrix.entry_names.size());
}
ROS_INFO("entry values");
for (auto den : ps2->allowed_collision_matrix.entry_values){
for (auto ene : den.enabled) ROS_INFO("%i", ene);
ROS_INFO("count %i", den.enabled.size());
}
ROS_INFO("[Robot 2] fft");
for (auto ft : ps2->fixed_frame_transforms){
ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq);
}
ROS_INFO("[Robot 2] is_diff");
ROS_INFO("is_diff: %i", ps2->is_diff);
ROS_INFO("[Robot 2] link padding");
for (auto lp : ps2->link_padding){
ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding);
}
ROS_INFO("[Robot 2] link scale");
for (auto ls : ps2->link_scale){
ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale);
}
ROS_INFO("[Robot 2] name");
ROS_INFO("name: %s", ps2->name.c_str());
ROS_INFO("[Robot 2] object color");
for (auto oc : ps2->object_colors){
ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str());
}
ROS_INFO("[Robot 2] rmn");
ROS_INFO("rmn %s", ps2->robot_model_name.c_str());
ROS_INFO("[Robot 2] rs");
for (auto aco : ps2->robot_state.attached_collision_objects){
ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq);
for (auto jn : aco.detach_posture.joint_names){
ROS_INFO("aco dp joint name: %s", jn.c_str());
}
ROS_INFO("aco dp Points");
for (auto p : aco.detach_posture.points){
for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa);
for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa);
ROS_INFO("aco dp points time from start");
for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa);
for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa);
}
ROS_INFO("aco link names %s", aco.link_name.c_str());
ROS_INFO("aco collis obj id %s", aco.object.id.c_str());
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str());
ROS_INFO("aco collis obj weight %f", aco.weight);
}
for (auto d : ps2->robot_state.joint_state.position) ROS_INFO("js position %f ", d);
for (auto d : ps2->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
for (auto d : ps2->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d);
for (auto d : ps2->robot_state.joint_state.effort) ROS_INFO(" js e %f", d);
ROS_INFO ("%s, %i", ps2->robot_state.joint_state.header.frame_id.c_str(), ps2->robot_state.joint_state.header.seq);
for (auto p : ps2->world.collision_objects) ROS_INFO("world co id %s", p.id.c_str());
ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps2->world.octomap.origin.position.x, ps2->world.octomap.origin.position.y, ps2->world.octomap.origin.position.z, ps2->world.octomap.origin.orientation.x, ps2->world.octomap.origin.orientation.y, ps2->world.octomap.origin.orientation.z, ps2->world.octomap.origin.orientation.w);
ROS_INFO("world om header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq);
ROS_INFO("world om Data");
}
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().c_str());
std::regex rx_box("bottle*");
std::regex rx_table("table*");
ROS_INFO("start merging");
// Iterate task collsion matrix
for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
// look for relevant fields
ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[j].c_str());
if( std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box) ||
std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links) ||
std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table) ||
mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
// Iterate over values
ROS_INFO("true");
for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
// filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant
ROS_INFO("start filtering");
if( std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links) ||
std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box) ||
std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table) ||
mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
std::string relevant_field;
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str();
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str();
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str();
if(mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str();
// => Update acm_
// calculate the distance in acm for name in in acm message ('cause they may differ, you know)
// totally not stolen from SO
ROS_INFO("calculate distance");
int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
// better filter with regex
// set acm value with respect to the position in acm
ROS_INFO("acm überschreiben mit distnace %i", distance);
ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str());
for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a);
ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]);
acm_.at(mr->map().at(relevant_field))[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str());
}
}
}
}
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().c_str());
std::regex rx_box("bottle*");
std::regex rx_table("table*");
ROS_INFO("start merging");
// Iterate task collsion matrix
for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
// look for relevant fields
ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[j].c_str());
if( std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box) ||
std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links) ||
std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table) ||
mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
// Iterate over values
ROS_INFO("true");
for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
// filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant
ROS_INFO("start filtering");
if( std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links) ||
std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box) ||
std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table) ||
mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
std::string relevant_field;
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str();
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str();
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str();
if(mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str();
// => Update acm_
// calculate the distance in acm for name in in acm message ('cause they may differ, you know)
// totally not stolen from SO
ROS_INFO("calculate distance");
int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
// better filter with regex
// set acm value with respect to the position in acm
ROS_INFO("acm überschreiben mit distnace %i", distance);
ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str());
for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a);
ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]);
acm_.at(mr->map().at(relevant_field))[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str());
}
}
}
}
merge_ps(ps_m, ps2, mr);
}
/*
if(!acm_m.empty()){
merge_acm(acm_m);
}
*/
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::merge_acm(moveit_msgs::PlanningScene& ps_m){
moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
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::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]);
}
}
}
/*
for( auto aco : in->robot_state.attached_collision_objects) {
out.robot_state.attached_collision_objects.push_back(aco);
}
for( auto aco : in->robot_state.joint_state) {
out.robot_state.attached_collision_objects.push_back(aco);
}
sensor_msgs::JointState::
for (auto str : res.allowed_collision_matrix.entry_names){
auto entry = std::find(origin->allowed_collision_matrix.entry_names.begin(), origin->allowed_collision_matrix.entry_names.end(), str);
if (entry != origin->allowed_collision_matrix.entry_names.end()){
origin->allowed_collision_matrix.entry_values[entry - origin->allowed_collision_matrix.entry_names.begin()]
}
}
*/
}
void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
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 = 0.9355f;
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));
}
// 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_;
}
void Moveit_mediator::rewrite_task_template(Abstract_robot* r, 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));
// unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up
std::string support_surface1 = "nichts";
std::string support_surface2 = "nichts";
XmlRpc::XmlRpcValue task;
nh_->getParam("task/stages", task);
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;
}
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
ROS_INFO("%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ());
ROS_INFO("surface1 %s", support_surface1.c_str());
ROS_INFO("surface2 %s", support_surface2.c_str());
nh_->setParam("/task/properties/group", r->name());
nh_->setParam("/task/properties/eef", mr->map()["eef"]);
nh_->setParam("/task/properties/hand_grasping_frame", mr->map()["hand_grasping_frame"]);
nh_->setParam("/task/properties/ik_frame", mr->map()["ik_frame"]);
nh_->setParam("/task/properties/hand", mr->map()["hand"]);
XmlRpc::XmlRpcValue a, b, c, d, e, h;
a["group"] = mr->map()["hand"];
e["joint_model_group_name"] = mr->map()["hand"];
b = task[4]["stages"];
b[1]["stage"]["properties"]["object"] = source.id;
b[3]["properties"] = a;
b[2]["set"]["allow_collisions"]["first"] = source.id;
b[2]["set"]["allow_collisions"]["second"] = e;
c = task[6]["stages"];
c[0]["set"]["allow_collisions"]["first"] = source.id;
c[0]["set"]["allow_collisions"]["second"] = support_surface2;
c[3]["properties"] = a;
c[6]["properties"] = a;
c[4]["set"]["allow_collisions"]["first"] = source.id; c[4]["set"]["allow_collisions"]["second"] = e;
c[2]["stage"]["set"]["pose"]["point"]["x"]= static_cast<double>(target.getOrigin().getX());
c[2]["stage"]["set"]["pose"]["point"]["y"]= static_cast<double>(target.getOrigin().getY());
c[2]["stage"]["set"]["pose"]["point"]["z"]= static_cast<double>(target.getOrigin().getZ());
task[2]["properties"] = a;
XmlRpc::XmlRpcValue connect, f, g;
f[r->name()] = "sampling";
g["source"] = "PARENT";
connect["type"] = "Connect";
connect["group_planner_vector"] = f;
connect["propertiesConfigureInitFrom"] = g;
task[5] = connect;
task[3] = connect;
a.clear();
e.clear();
a["link"] = mr->map()["ik_frame"];
a["min_distance"] = 0.07;
a["max_distance"] = 0.2;
c[5]["properties"] = a;
b[0]["properties"] = a;
a["min_distance"] = 0.1;
a["max_distance"] = 0.2;
c[1]["properties"] = a;
e["object"] = "bottle";
e["link"] = mr->map()["ik_frame"];
b[4]["set"]["attach_object"] = e;
c[4]["set"]["detach_object"] = e;
c[2]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
c[5]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"];
b[1]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
b[6]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
b[0]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"];
b[5]["set"]["allow_collisions"]["first"] = source.id;
b[5]["set"]["allow_collisions"]["second"] = support_surface1;
b[7]["set"]["allow_collisions"]["first"] = source.id;
b[7]["set"]["allow_collisions"]["second"] = support_surface1;
task[6]["stages"] = c;
task[4]["stages"] = b;
nh_->setParam("task/stages", 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")){
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);
};