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

starting docker

parent 264eb6ff
No related branches found
No related tags found
No related merge requests found
Pipeline #14869 failed
Robot_arm1:
pos_x: -2.041459083557129e-06
pos_y: 0.09999796003103256
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.249713353108504e-06
q_w: 0.9999999999992193
rpy: 0.000000 -0.000000 0.000002
wings: 5
Robot_arm2:
pos_x: -6.040541873092198e-06
pos_y: 1.699997983867894
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.337136133234621e-06
q_w: 0.9999999999991062
rpy: 0.000000 -0.000000 0.000003
wings: 6
\ No newline at end of file
Robot_arm1:
pos_x: -2.041459083557129e-06
pos_y: -0.1000020429491997
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.249713353108504e-06
q_w: 0.9999999999992193
rpy: 0.000000 -0.000000 0.000002
wings: 7
Robot_arm2:
pos_x: -6.040541873092198e-06
pos_y: 1.499997980887662
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.337136133234621e-06
q_w: 0.9999999999991062
rpy: 0.000000 -0.000000 0.000003
wings: 5
\ No newline at end of file
Robot_arm1:
pos_x: -2.041459083557129e-06
pos_y: 0.09999796003103256
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.249713353108504e-06
q_w: 0.9999999999992193
rpy: 0.000000 -0.000000 0.000002
wings: 5
Robot_arm2:
pos_x: -6.040541873092198e-06
pos_y: 1.699997983867894
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.337136133234621e-06
q_w: 0.9999999999991062
rpy: 0.000000 -0.000000 0.000003
wings: 7
\ No newline at end of file
Robot_arm1:
pos_x: -2.041459083557129e-06
pos_y: -0.1000020429491997
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.249713353108504e-06
q_w: 0.9999999999992193
rpy: 0.000000 -0.000000 0.000002
wings: 7
Robot_arm2:
pos_x: -6.040541873092198e-06
pos_y: 1.499997980887662
pos_z: 0.4424999952316284
q_x: 0
q_y: 0
q_z: 1.337136133234621e-06
q_w: 0.9999999999991062
rpy: 0.000000 -0.000000 0.000003
wings: 7
\ No newline at end of file
...@@ -52,12 +52,14 @@ int main(int argc, char **argv){ ...@@ -52,12 +52,14 @@ int main(int argc, char **argv){
Abstract_robot* robo2 = new Robot(std::string("Robot_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f))); Abstract_robot* robo2 = new Robot(std::string("Robot_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)));
Robot* ceti2 = dynamic_cast<Robot*>(robo2); Robot* ceti2 = dynamic_cast<Robot*>(robo2);
ceti2->set_observer_mask(wing_config::RML);
ceti2->add_rviz_markers(wings2); ceti2->add_rviz_markers(wings2);
ceti2->add_rviz_markers(fields2); ceti2->add_rviz_markers(fields2);
mediator->connect_robots(robo2); mediator->connect_robots(robo2);
mediator->mediate(); mediator->mediate();
/* /*
for (int i = 0; i < ceti->access_fields1().size(); i++){ for (int i = 0; i < ceti->access_fields1().size(); i++){
ceti->access_fields1()[i] = new Field_rviz_decorator(ceti->access_fields1()[i], fields1); ceti->access_fields1()[i] = new Field_rviz_decorator(ceti->access_fields1()[i], fields1);
......
...@@ -14,6 +14,28 @@ int main(int argc, char **argv){ ...@@ -14,6 +14,28 @@ int main(int argc, char **argv){
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
float x1,y1,z1,qx1, qy1, qz1, qw1;
float x2,y2,z2,qx2, qy2, qz2, qw2;
int a,b;
nh.getParam("/Robot_arm1/pos_x",x1);
nh.getParam("/Robot_arm1/pos_y",y1);
nh.getParam("/Robot_arm1/pos_z",z1);
nh.getParam("/Robot_arm1/q_x",qx1);
nh.getParam("/Robot_arm1/q_y",qy1);
nh.getParam("/Robot_arm1/q_z",qz1);
nh.getParam("/Robot_arm1/q_w",qw1);
nh.getParam("/Robot_arm1/wings",a);
nh.getParam("/Robot_arm2/pos_x",x2);
nh.getParam("/Robot_arm2/pos_y",y2);
nh.getParam("/Robot_arm2/pos_z",z2);
nh.getParam("/Robot_arm2/q_x",qx2);
nh.getParam("/Robot_arm2/q_y",qy2);
nh.getParam("/Robot_arm2/q_z",qz2);
nh.getParam("/Robot_arm2/q_w",qw2);
nh.getParam("/Robot_arm2/wings",b);
std::vector<std::vector<tf2::Transform>> objects; //in progress std::vector<std::vector<tf2::Transform>> objects; //in progress
ros::Publisher* pub = new ros::Publisher(nh.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor ros::Publisher* pub = new ros::Publisher(nh.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor
...@@ -22,8 +44,12 @@ int main(int argc, char **argv){ ...@@ -22,8 +44,12 @@ int main(int argc, char **argv){
moveit_mediator->load_robot_description(); moveit_mediator->load_robot_description();
Abstract_robot* robo1 = new Moveit_robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,0,0.4425f))); ROS_INFO("%f %f %f %f %f %f %f", x1,y1,z1,qx1, qy1, qz1, qw1);
Abstract_robot* robo2 = new Moveit_robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-1,0,0.4425f))); ROS_INFO("%f %f %f %f %f %f %f", x2,y2,z2,qx2, qy2, qz2, qw2);
Abstract_robot* robo1 = new Moveit_robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(qx1,qy1,qz1,qw1), tf2::Vector3(x1,y1,0.4425f)));
Abstract_robot* robo2 = new Moveit_robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(qx2,qy2,qz2,qw2), tf2::Vector3(x2,y2,0.4425f)));
robo1->set_observer_mask(wing_config::RML); robo1->set_observer_mask(wing_config::RML);
robo2->set_observer_mask(wing_config::RML); robo2->set_observer_mask(wing_config::RML);
...@@ -38,9 +64,10 @@ int main(int argc, char **argv){ ...@@ -38,9 +64,10 @@ int main(int argc, char **argv){
Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robo2); Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robo2);
std::bitset<3> i = 7; std::bitset<3> ia = a;
moveit_mediator->build_wings(i, ceti1); std::bitset<3> ib = b;
moveit_mediator->build_wings(i, ceti2); moveit_mediator->build_wings(ia, ceti1);
moveit_mediator->build_wings(ib, ceti2);
ceti1->notify(); ceti1->notify();
ceti2->notify(); ceti2->notify();
......
...@@ -5,10 +5,10 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) { ...@@ -5,10 +5,10 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) {
ROS_INFO("starting base calculation strategy..."); ROS_INFO("starting base calculation strategy...");
ROS_INFO("condition based [inv_map] calculation..."); ROS_INFO("condition based [inv_map] calculation...");
std::vector<tf2::Transform> trans; std::vector<tf2::Transform> trans;
for(int i = 0; i < var->map().size(); i++) { for(long unsigned int i = 0; i < var->map().size(); i++) {
for (int x = 0; x < var->target_rot().size(); x++) { for (long unsigned int x = 0; x < var->target_rot().size(); x++) {
for (int y = 0; y < var->target_rot()[x].size(); y++) { for (long unsigned int y = 0; y < var->target_rot()[x].size(); y++) {
for(int p = 0; p < var->target_rot()[x][y].size(); p++){ for(long unsigned int p = 0; p < var->target_rot()[x][y].size(); p++){
if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.349066f) { if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.349066f) {
trans.push_back(tf2::Transform(var->target_rot()[x][y][p], var->map()[i].getOrigin()).inverse()); trans.push_back(tf2::Transform(var->target_rot()[x][y][p], var->map()[i].getOrigin()).inverse());
break; break;
...@@ -18,7 +18,7 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) { ...@@ -18,7 +18,7 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) {
} }
} }
var->set_inv_map(trans); var->set_inv_map(trans);
ROS_INFO("caculated [inv_map] contains %i entrys...", var->inv_map().size()); ROS_INFO("caculated [inv_map] contains %li entrys...", var->inv_map().size());
}; };
void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) { void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
...@@ -27,8 +27,8 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) { ...@@ -27,8 +27,8 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
// Optimize targets in later implementation // Optimize targets in later implementation
std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_clouds; std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_clouds;
target_clouds.resize(var->task_grasps().size()); target_clouds.resize(var->task_grasps().size());
for(int i = 0; i < target_clouds.size(); i++) { for(long unsigned int i = 0; i < target_clouds.size(); i++) {
for (int j = 0; j < var->task_grasps()[i].size();j++) { for (long unsigned int j = 0; j < var->task_grasps()[i].size();j++) {
target_clouds[i].push_back(pcl::PointCloud< pcl::PointXYZ >::Ptr(new pcl::PointCloud< pcl::PointXYZ >)); target_clouds[i].push_back(pcl::PointCloud< pcl::PointXYZ >::Ptr(new pcl::PointCloud< pcl::PointXYZ >));
} }
} }
...@@ -36,10 +36,10 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) { ...@@ -36,10 +36,10 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
// Maybe OpenMP // Maybe OpenMP
ROS_INFO("start [cloud] calculation..."); ROS_INFO("start [cloud] calculation...");
tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)); tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
for (int i = 0; i < target_clouds.size(); i++){ for (long unsigned int i = 0; i < target_clouds.size(); i++){
for (int j = 0; j < target_clouds[i].size(); j++) { for (long unsigned int j = 0; j < target_clouds[i].size(); j++) {
for (int y = 0; y < var->inv_map().size(); y++) { for (long unsigned int y = 0; y < var->inv_map().size(); y++) {
for (int x = 0; x < var->target_rot()[i][j].size(); x++) { for (long unsigned int x = 0; x < var->target_rot()[i][j].size(); x++) {
tf2::Transform target = var->task_grasps()[i][j]; target.setRotation(var->target_rot()[i][j][x]); tf2::Transform target = var->task_grasps()[i][j]; target.setRotation(var->target_rot()[i][j][x]);
tf2::Transform base = target * (var->inv_map()[y] * root); tf2::Transform base = target * (var->inv_map()[y] * root);
target_clouds[i][j]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ())); target_clouds[i][j]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
......
...@@ -17,7 +17,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){ ...@@ -17,7 +17,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
_map.push_back(tf2::Transform(so.normalize(),t)); _map.push_back(tf2::Transform(so.normalize(),t));
} }
this->set_map(_map); this->set_map(_map);
ROS_INFO("[map] saved with %i entries...", _map.size()); ROS_INFO("[map] saved with %li entries...", _map.size());
ROS_INFO("saving [target] positions..."); ROS_INFO("saving [target] positions...");
std::vector<std::vector<tf2::Transform>> task_; std::vector<std::vector<tf2::Transform>> task_;
...@@ -33,7 +33,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){ ...@@ -33,7 +33,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
} }
this->set_task_grasps(task_); this->set_task_grasps(task_);
for (int i = 0; i < task_.size(); i++) ROS_INFO("[target] for Robot %i saved with %i entries...", i, task_[i].size()); for (long unsigned int i = 0; i < task_.size(); i++) ROS_INFO("[target] for Robot %li saved with %li entries...", i, task_[i].size());
} }
std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
...@@ -50,9 +50,9 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){ ...@@ -50,9 +50,9 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
basic_rot.push_back(z_rot.inverse().normalize()); basic_rot.push_back(z_rot.inverse().normalize());
std::vector<std::vector<std::vector<tf2::Quaternion>>> target_orientation_grasps; std::vector<std::vector<std::vector<tf2::Quaternion>>> target_orientation_grasps;
for (int i = 0; i < task_grasps_.size(); i++) { for (long unsigned int i = 0; i < task_grasps_.size(); i++) {
std::vector<std::vector<tf2::Quaternion>> quat; std::vector<std::vector<tf2::Quaternion>> quat;
for (int j = 0; j < task_grasps_[i].size(); j++) { for (long unsigned int j = 0; j < task_grasps_[i].size(); j++) {
quat.push_back(basic_rot); quat.push_back(basic_rot);
} }
target_orientation_grasps.push_back(quat); target_orientation_grasps.push_back(quat);
...@@ -66,21 +66,21 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){ ...@@ -66,21 +66,21 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
std::vector<pcl::PointXYZ> voxelization = this->create_pcl_box(); std::vector<pcl::PointXYZ> voxelization = this->create_pcl_box();
std::vector<std::vector<std::vector<int>>> base_target_map; std::vector<std::vector<std::vector<int>>> base_target_map;
base_target_map.resize(task_grasps_.size()); base_target_map.resize(task_grasps_.size());
for(int i = 0; i < base_target_map.size();i++) base_target_map[i].resize(voxelization.size()); for(long unsigned int i = 0; i < base_target_map.size();i++) base_target_map[i].resize(voxelization.size());
ROS_INFO("forming base clouds..."); ROS_INFO("forming base clouds...");
strategy_->cloud_calculation(this); strategy_->cloud_calculation(this);
// OpenMP // OpenMP
ROS_INFO("start cloud quantization..."); ROS_INFO("start cloud quantization...");
for(int i = 0; i < target_cloud_.size();i++){ for(long unsigned int i = 0; i < target_cloud_.size();i++){
for(int j = 0; j < target_cloud_[i].size();j++){ for(long unsigned int j = 0; j < target_cloud_[i].size();j++){
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f); pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f);
octree.setInputCloud(target_cloud_[i][j]); octree.setInputCloud(target_cloud_[i][j]);
octree.addPointsFromInputCloud(); octree.addPointsFromInputCloud();
double min_x, min_y, min_z, max_x, max_y, max_z; double min_x, min_y, min_z, max_x, max_y, max_z;
octree.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); octree.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
for(int k = 0; k < voxelization.size(); k++) { for(long unsigned int k = 0; k < voxelization.size(); k++) {
pcl::PointXYZ p = voxelization[k]; pcl::PointXYZ p = voxelization[k];
bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z); bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
if(isInBox && octree.isVoxelOccupiedAtPoint(p)) { if(isInBox && octree.isVoxelOccupiedAtPoint(p)) {
...@@ -96,7 +96,7 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){ ...@@ -96,7 +96,7 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
std::vector<std::vector<pcl::PointXYZ>> resulting; std::vector<std::vector<pcl::PointXYZ>> resulting;
for(int i = 0; i < base_target_map.size(); i++) { for(long unsigned int i = 0; i < base_target_map.size(); i++) {
std::vector<pcl::PointXYZ> points_per_robot; std::vector<pcl::PointXYZ> points_per_robot;
for(int j = 0; j < base_target_map[i].size(); j++){ for(int j = 0; j < base_target_map[i].size(); j++){
if (base_target_map[i][j].size() == task_grasps_[i].size()) { if (base_target_map[i][j].size() == task_grasps_[i].size()) {
...@@ -106,8 +106,8 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){ ...@@ -106,8 +106,8 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
if (!points_per_robot.empty()) resulting.push_back(points_per_robot); if (!points_per_robot.empty()) resulting.push_back(points_per_robot);
} }
for (int i = 0; i < resulting.size(); i++) { for (long unsigned int i = 0; i < resulting.size(); i++) {
ROS_INFO("Robot %i got %i base positions to ckeck", i, resulting[i].size()); ROS_INFO("Robot %li got %li base positions to ckeck", i, resulting[i].size());
} }
return resulting; return resulting;
} }
......
#include "impl/mediator.h" #include "impl/mediator.h"
#include "impl/wing_rviz_decorator.h" #include "impl/wing_rviz_decorator.h"
#include <tf2/LinearMath/Scalar.h>
bool Mediator::check_collision( const int& robot){ bool Mediator::check_collision( const int& robot){
bool succ = true; bool succ = true;
for(int j = 0; j < objects_[robot].size(); j++){ for(long unsigned int j = 0; j < objects_[robot].size(); j++){
visualization_msgs::Marker marker; visualization_msgs::Marker marker;
marker.header.frame_id = "map"; marker.header.frame_id = "map";
marker.header.stamp = ros::Time(); marker.header.stamp = ros::Time();
...@@ -39,9 +40,9 @@ bool Mediator::check_collision( const int& robot){ ...@@ -39,9 +40,9 @@ bool Mediator::check_collision( const int& robot){
rviz_markers_->markers.push_back(marker); rviz_markers_->markers.push_back(marker);
} }
return succ; return succ;
} }
void Mediator::mediate(){ void Mediator::mediate(){
ROS_INFO("assigne result to first robot..."); ROS_INFO("assigne result to first robot...");
std::vector<pcl::PointXYZ> grCenter = Abstract_mediator::generate_Ground(tf2::Vector3(0,0,0), 3.0f, 0.1f); std::vector<pcl::PointXYZ> grCenter = Abstract_mediator::generate_Ground(tf2::Vector3(0,0,0), 3.0f, 0.1f);
...@@ -61,121 +62,114 @@ void Mediator::mediate(){ ...@@ -61,121 +62,114 @@ void Mediator::mediate(){
} }
} }
for (int i = objects_[0].size()-1; i > 0; i--){ for (long unsigned int i = objects_[0].size()-1; i > 0; i--){
if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back(); if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back();
} }
std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>> dict;
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]); Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
for(int j = 0; j <= 7; j++){ for(int j = 0; j <= 7; j++){
std::bitset<3> wing_config(j); std::bitset<3> wing_config(j);
build_wings(wing_config, ceti1); build_wings(wing_config, ceti1);
for (int i = 0; i < ground_per_robot.size(); i++){ for (long unsigned int i = 0; i < ground_per_robot.size(); i++){
robots_[0]->set_tf(ground_per_robot[i]); robots_[0]->set_tf(ground_per_robot[i]);
for ( float p = 0; p < 2*M_PI; p += 0.0872665f){ for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear(); for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear();
rviz_markers_->markers.clear(); rviz_markers_->markers.clear();
ceti1->rotate(0.0872665f); ceti1->rotate(0.0872665f);
ceti1->notify(); ceti1->notify();
bool temp = check_collision(0);
/*
if (check_collision(0)) { if (check_collision(0)) {
approximation(ceti1, j, dict); approximation(ceti1, j);
} else { } else {
continue; continue;
} }
*/
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) pub_->publish(*markers); for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) pub_->publish(*markers);
pub_->publish(*rviz_markers_); pub_->publish(*rviz_markers_);
} }
} }
ceti1->reset(); ceti1->reset();
} }
if (dict.empty()){
ROS_INFO("unfortunately, no result is found...");
return;
} else {
ROS_INFO("write data to file...");
} }
} void Mediator::approximation(Robot* robot, int& wings){
ROS_INFO("assigne result to first robot...");
void Mediator::approximation(Robot* robot, int& wings, std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict){
ros::Rate loop_rate(1);
std::vector<pcl::PointXYZ> relative_ground;
pcl::octree::OctreePointCloudSearch< pcl::PointXYZ > cloud(0.4f);
cloud.setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[1]));
cloud.addPointsFromInputCloud();
Robot* ceti = dynamic_cast<Robot*>(robots_[1]); Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
std::vector<std::pair<tf2::Transform, int>> second_robot_data;
for (int i = 0; i <= 7; i++){ for (int i = 0; i <= 7; i++){
std::bitset<3> wing_config(i); std::bitset<3> wing_config(i);
build_wings(wing_config, ceti); build_wings(wing_config, ceti);
for (Abstract_robot_element* fields : robot->access_fields()) { for (Abstract_robot_element* fields : robot->access_fields()) {
ceti->set_tf(fields->world_tf()); ceti->set_tf(fields->world_tf());
for ( float p = 0; p < 2*M_PI; p += M_PI/2){ for ( float p = 0; p < 2*M_PI; p += M_PI/2){
for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) markers->markers.clear();
for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
rviz_markers_->markers.clear();
ceti->rotate(M_PI/2); ceti->rotate(M_PI/2);
ceti->notify(); ceti->notify();
if (robot->check_robot_collision(ceti)) continue;
if (check_collision(1)) { if (check_collision(1)) {
ROS_INFO("should work"); ROS_INFO("should work");
second_robot_data.push_back(std::pair<tf2::Transform, int>(ceti->tf(), i)); write_file(robot, wings, ceti, i);
} else { } else {
continue; continue;
} }
loop_rate.sleep(); for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) pub_->publish(*markers);
for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) pub_->publish(*markers);
pub_->publish(*rviz_markers_);
} }
//relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ())); //relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ()));
} }
ceti->reset(); ceti->reset();
} }
std::pair<tf2::Transform, int> ceti1(robot->tf(), wings);
dict.push_back(std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>(ceti1, second_robot_data));
ceti->reset();
} }
void Mediator::write_file(std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict){
/*
std::ofstream o(ros::package::getPath("mtc") + "/results/dummy.yaml"); void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
YAML::Node node; YAML::Node node;
std::ofstream o(ros::package::getPath("mtc") + "/results/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
double r,p,y;
tf2::Matrix3x3 m(A->tf().getRotation());
m.getRPY(r,p,y);
for(tf2::Vector3& vec : robot_1){
node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ())); node[A->name()]["pos_x"] = A->tf().getOrigin().getX();
} node[A->name()]["pos_y"] = A->tf().getOrigin().getY();
node[A->name()]["pos_z"] = A->tf().getOrigin().getZ();
ROS_INFO("writing items for robot2..."); node[A->name()]["q_x"] = A->tf().getRotation().getX();
for(tf2::Vector3& vec : robot_2){ node[A->name()]["q_y"] = A->tf().getRotation().getY();
node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ())); node[A->name()]["q_z"] = A->tf().getRotation().getZ();
} node[A->name()]["q_w"] = A->tf().getRotation().getW();
node[A->name()]["rpy"] = std::to_string(r) + " " + std::to_string(p) + " " + std::to_string(y);
ROS_INFO("writing intersection..."); node[A->name()]["wings"] = a;
for(tf2::Vector3& vec : robot_middle){
node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; m.setRotation(B->tf().getRotation());
node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; m.getRPY(r,p,y);
}
node[B->name()]["pos_x"] = B->tf().getOrigin().getX();
node[B->name()]["pos_y"] = B->tf().getOrigin().getY();
node[B->name()]["pos_z"] = B->tf().getOrigin().getZ();
node[B->name()]["q_x"] = B->tf().getRotation().getX();
node[B->name()]["q_y"] = B->tf().getRotation().getY();
node[B->name()]["q_z"] = B->tf().getRotation().getZ();
node[B->name()]["q_w"] = B->tf().getRotation().getW();
node[B->name()]["rpy"] = std::to_string(r) + " " + std::to_string(p) + " " + std::to_string(y);
node[B->name()]["wings"] = b;
o << node; o << node;
*/ o.close();
} }
void Mediator::build_wings(std::bitset<3>& wing, Robot* robot){ void Mediator::build_wings(std::bitset<3>& wing, Robot* robot){
std::bitset<3> result = robot->observer_mask() & wing; std::bitset<3> result = robot->observer_mask() & wing;
for (int i = 0; i < result.size(); i++){ for (std::size_t i = 0; i < result.size(); i++){
ROS_INFO( "%i", i);
if (result[i]){ if (result[i]){
Abstract_robot_element* moveit_right = new Wing_rviz_decorator(new Wing(wings_[i].pos_, wings_[i].size_), robot->rviz_markers()[0]); Abstract_robot_element* moveit_right = new Wing_rviz_decorator(new Wing(wings_[i].pos_, wings_[i].size_), robot->rviz_markers()[0]);
robot->register_observers(moveit_right); robot->register_observers(moveit_right);
......
...@@ -8,7 +8,7 @@ void Moveit_mediator::publish_tables(){ ...@@ -8,7 +8,7 @@ void Moveit_mediator::publish_tables(){
moveit_msgs::PlanningScene planning_scene; moveit_msgs::PlanningScene planning_scene;
planning_scene.is_diff = true; planning_scene.is_diff = true;
for(int i = 0; i < robots_.size();i++){ for(long unsigned int i = 0; i < robots_.size();i++){
Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]); Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
for (moveit_msgs::CollisionObject* markers : ceti->coll_markers()) { for (moveit_msgs::CollisionObject* markers : ceti->coll_markers()) {
planning_scene.world.collision_objects.push_back(*markers); planning_scene.world.collision_objects.push_back(*markers);
...@@ -32,7 +32,7 @@ void Moveit_mediator::mediate(){}; ...@@ -32,7 +32,7 @@ void Moveit_mediator::mediate(){};
void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){ void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){
std::bitset<3> result = robot->observer_mask() & wing; std::bitset<3> result = robot->observer_mask() & wing;
for (int i = 0; i < result.size(); i++){ for (std::size_t i = 0; i < result.size(); i++){
if (result[i]){ if (result[i]){
moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject(); moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject();
robot->add_coll_markers(marker); robot->add_coll_markers(marker);
......
...@@ -42,18 +42,14 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){ ...@@ -42,18 +42,14 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){
if ((std::floor(sum*100)/100.f) <= full_area) {return false; } if ((std::floor(sum*100)/100.f) <= full_area) {return false; }
for (int i = 0; i < bounds_.size(); i++){ A = tf_ * bounds_[0];
tf2::Transform A = tf_ * bounds_[0]; B = tf_ * bounds_[1];
tf2::Transform B = tf_ * bounds_[1]; C = tf_ * bounds_[2];
tf2::Transform C = tf_ * bounds_[2]; D = tf_ * bounds_[3];
tf2::Transform D = tf_ * bounds_[3];
full_area = area_calculation(A,B,C) + area_calculation(A,D,C); full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A); sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
if ((std::floor(sum*100)/100.f) <= full_area) return true; if ((std::floor(sum*100)/100.f) <= full_area) return true;
}
if (!observers_.empty()){ if (!observers_.empty()){
std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin(); std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
...@@ -81,14 +77,77 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){ ...@@ -81,14 +77,77 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){
void Robot::reset(){ void Robot::reset(){
observers_.clear(); observers_.clear();
access_fields_.clear(); access_fields_.clear();
generate_access_fields(); generate_access_fields();
tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)); tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
} }
void Robot::notify(){ void Robot::notify(){
for(Abstract_robot_element* wing : observers_) wing->update(tf_); for(Abstract_robot_element* field : access_fields_) field->update(tf_); for(Abstract_robot_element* wing : observers_) wing->update(tf_);
for(Abstract_robot_element* field : access_fields_) field->update(tf_);
}
bool Robot::check_robot_collision( Robot* rob){
tf2::Transform A = tf_ * bounds_[0];
tf2::Transform B = tf_ * bounds_[1];
tf2::Transform C = tf_ * bounds_[2];
tf2::Transform D = tf_ * bounds_[3];
float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
float sum = area_calculation(A,rob->tf(),D) + area_calculation(D,rob->tf(),C) + area_calculation(C,rob->tf(),B) + area_calculation(rob->tf(), B, A);
if ((std::floor(sum*100)/100.f) <= full_area) return true;
if (!rob->observers().empty()){
std::vector<Abstract_robot_element*>::const_iterator it = rob->observers().begin();
while (it != rob->observers().end()) {
Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
sum = area_calculation(A,deco->wing()->world_tf(),D) + area_calculation(D,deco->wing()->world_tf(),C) + area_calculation(C,deco->wing()->world_tf(),B) + area_calculation(deco->wing()->world_tf(), B, A);
if ((std::floor(sum*100)/100.f) <= full_area) {
return true;
} else {
++it;
}
}
}
if (!observers_.empty()){
std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
while (it != observers_.end()) {
Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
Wing* ceti = dynamic_cast<Wing*>(deco->wing());
tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
sum = area_calculation(A,rob->tf(),D) + area_calculation(D,rob->tf(),C) + area_calculation(C,rob->tf(),B) + area_calculation(rob->tf(), B, A);
if ((std::floor(sum*100)/100.f) <= full_area) return true;
if (!rob->observers().empty()){
std::vector<Abstract_robot_element*>::const_iterator itt = rob->observers().begin();
while (itt != rob->observers().end()) {
Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*itt);
sum = area_calculation(A,deco->wing()->world_tf(),D) + area_calculation(D,deco->wing()->world_tf(),C) + area_calculation(C,deco->wing()->world_tf(),B) + area_calculation(deco->wing()->world_tf(), B, A);
if ((std::floor(sum*100)/100.f) <= full_area) {
return true;
} else {
++itt;
} }
}
}
if ((std::floor(sum*100)/100.f) <= full_area) {
return true;
} else {
++it;
}
}
}
return false;
}
void Robot::register_observers(Abstract_robot_element* wd){ void Robot::register_observers(Abstract_robot_element* wd){
...@@ -99,7 +158,7 @@ void Robot::register_observers(Abstract_robot_element* wd){ ...@@ -99,7 +158,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
int index = 0; int index = 0;
if (decorator->wing()->relative_tf().getOrigin().getY()>0){ if (decorator->wing()->relative_tf().getOrigin().getY()>0){
for(int i = 0; i < access_fields_.size(); i++){ for(long unsigned int i = 0; i < access_fields_.size(); i++){
if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){ if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
found = true; found = true;
index = i; index = i;
...@@ -108,7 +167,7 @@ void Robot::register_observers(Abstract_robot_element* wd){ ...@@ -108,7 +167,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
} }
if(found) {access_fields_.erase(access_fields_.begin() + index); return;} if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
} else if (decorator->wing()->relative_tf().getOrigin().getY()<0) { } else if (decorator->wing()->relative_tf().getOrigin().getY()<0) {
for(int i = 0; i < access_fields_.size(); i++){ for(long unsigned int i = 0; i < access_fields_.size(); i++){
if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){ if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
found = true; found = true;
index = i; index = i;
...@@ -117,7 +176,7 @@ void Robot::register_observers(Abstract_robot_element* wd){ ...@@ -117,7 +176,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
} }
if(found) {access_fields_.erase(access_fields_.begin() + index); return;} if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
} else { } else {
for(int i = 0; i < access_fields_.size(); i++){ for(long unsigned int i = 0; i < access_fields_.size(); i++){
if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){ if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){
found = true; found = true;
index = i; index = i;
......
...@@ -34,4 +34,7 @@ void Wing_moveit_decorator::output_filter() { ...@@ -34,4 +34,7 @@ void Wing_moveit_decorator::output_filter() {
markers_->primitive_poses[0].orientation.w = world_quat.getW(); markers_->primitive_poses[0].orientation.w = world_quat.getW();
markers_->operation = markers_->ADD; markers_->operation = markers_->ADD;
ROS_INFO("%f %f %f %f", world_quat.getX(), world_quat.getY(), world_quat.getZ(), world_quat.getW());
} }
\ No newline at end of file
...@@ -81,7 +81,7 @@ void Wing_rviz_decorator::output_filter() { ...@@ -81,7 +81,7 @@ void Wing_rviz_decorator::output_filter() {
bounds.color.a = 1.0; bounds.color.a = 1.0;
bounds.pose.orientation.w = 1; bounds.pose.orientation.w = 1;
for (int i = 0; i < wing->bounds().size(); i++){ for (long unsigned int i = 0; i < wing->bounds().size(); i++){
tf2::Transform point_posistion = wing->world_tf() * wing->bounds()[i]; tf2::Transform point_posistion = wing->world_tf() * wing->bounds()[i];
geometry_msgs::Point point; geometry_msgs::Point point;
point.x = point_posistion.getOrigin().getX(); point.x = point_posistion.getOrigin().getX();
......
#include "../include/mtc2taskspace.h" #include "../include/mtc2taskspace.h"
int main(int argc, char **argv){ int main(int argc, char **argv){
ros::init(argc, argv, "mtc2taskspace"); ros::init(argc, argv, "mtc2taskspace");
ros::NodeHandle n; ros::NodeHandle n;
...@@ -58,7 +60,7 @@ int main(int argc, char **argv){ ...@@ -58,7 +60,7 @@ int main(int argc, char **argv){
// lets make some result data // lets make some result data
ROS_INFO("writing file..."); ROS_INFO("writing file...");
std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy.yaml"); std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy2.yaml");
YAML::Node node; YAML::Node node;
ROS_INFO("writing items for robot1..."); ROS_INFO("writing items for robot1...");
...@@ -77,6 +79,15 @@ int main(int argc, char **argv){ ...@@ -77,6 +79,15 @@ int main(int argc, char **argv){
node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
} }
ROS_INFO("writing intersection...");
for(tf2::Vector3& vec : robot_middle){
node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
}
node.SetStyle(YAML::EmitterStyle::Flow);
o << node; o << node;
return 0; return 0;
} }
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment