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

starting docker

parent 264eb6ff
Branches
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){
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);
ceti2->set_observer_mask(wing_config::RML);
ceti2->add_rviz_markers(wings2);
ceti2->add_rviz_markers(fields2);
mediator->connect_robots(robo2);
mediator->mediate();
/*
for (int i = 0; i < ceti->access_fields1().size(); i++){
ceti->access_fields1()[i] = new Field_rviz_decorator(ceti->access_fields1()[i], fields1);
......
......@@ -14,6 +14,28 @@ int main(int argc, char **argv){
ros::AsyncSpinner spinner(1);
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
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){
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)));
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", x1,y1,z1,qx1, qy1, qz1, qw1);
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);
robo2->set_observer_mask(wing_config::RML);
......@@ -38,9 +64,10 @@ int main(int argc, char **argv){
Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robo2);
std::bitset<3> i = 7;
moveit_mediator->build_wings(i, ceti1);
moveit_mediator->build_wings(i, ceti2);
std::bitset<3> ia = a;
std::bitset<3> ib = b;
moveit_mediator->build_wings(ia, ceti1);
moveit_mediator->build_wings(ib, ceti2);
ceti1->notify();
ceti2->notify();
......
......@@ -5,10 +5,10 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) {
ROS_INFO("starting base calculation strategy...");
ROS_INFO("condition based [inv_map] calculation...");
std::vector<tf2::Transform> trans;
for(int i = 0; i < var->map().size(); i++) {
for (int x = 0; x < var->target_rot().size(); x++) {
for (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 i = 0; i < var->map().size(); i++) {
for (long unsigned int x = 0; x < var->target_rot().size(); x++) {
for (long unsigned int y = 0; y < var->target_rot()[x].size(); y++) {
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) {
trans.push_back(tf2::Transform(var->target_rot()[x][y][p], var->map()[i].getOrigin()).inverse());
break;
......@@ -18,7 +18,7 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) {
}
}
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) {
......@@ -27,8 +27,8 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
// Optimize targets in later implementation
std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_clouds;
target_clouds.resize(var->task_grasps().size());
for(int i = 0; i < target_clouds.size(); i++) {
for (int j = 0; j < var->task_grasps()[i].size();j++) {
for(long unsigned int i = 0; i < target_clouds.size(); i++) {
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 >));
}
}
......@@ -36,10 +36,10 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
// Maybe OpenMP
ROS_INFO("start [cloud] calculation...");
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 (int j = 0; j < target_clouds[i].size(); j++) {
for (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 i = 0; i < target_clouds.size(); i++){
for (long unsigned int j = 0; j < target_clouds[i].size(); j++) {
for (long unsigned int y = 0; y < var->inv_map().size(); y++) {
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 base = target * (var->inv_map()[y] * root);
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){
_map.push_back(tf2::Transform(so.normalize(),t));
}
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...");
std::vector<std::vector<tf2::Transform>> task_;
......@@ -33,7 +33,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& 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(){
......@@ -50,9 +50,9 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
basic_rot.push_back(z_rot.inverse().normalize());
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;
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);
}
target_orientation_grasps.push_back(quat);
......@@ -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<std::vector<std::vector<int>>> base_target_map;
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...");
strategy_->cloud_calculation(this);
// OpenMP
ROS_INFO("start cloud quantization...");
for(int i = 0; i < target_cloud_.size();i++){
for(int j = 0; j < target_cloud_[i].size();j++){
for(long unsigned int i = 0; i < target_cloud_.size();i++){
for(long unsigned int j = 0; j < target_cloud_[i].size();j++){
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f);
octree.setInputCloud(target_cloud_[i][j]);
octree.addPointsFromInputCloud();
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);
for(int k = 0; k < voxelization.size(); k++) {
for(long unsigned int k = 0; k < voxelization.size(); 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);
if(isInBox && octree.isVoxelOccupiedAtPoint(p)) {
......@@ -96,7 +96,7 @@ std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
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;
for(int j = 0; j < base_target_map[i].size(); j++){
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(){
if (!points_per_robot.empty()) resulting.push_back(points_per_robot);
}
for (int i = 0; i < resulting.size(); i++) {
ROS_INFO("Robot %i got %i base positions to ckeck", i, resulting[i].size());
for (long unsigned int i = 0; i < resulting.size(); i++) {
ROS_INFO("Robot %li got %li base positions to ckeck", i, resulting[i].size());
}
return resulting;
}
......
#include "impl/mediator.h"
#include "impl/wing_rviz_decorator.h"
#include <tf2/LinearMath/Scalar.h>
bool Mediator::check_collision( const int& robot){
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;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
......@@ -39,9 +40,9 @@ bool Mediator::check_collision( const int& robot){
rviz_markers_->markers.push_back(marker);
}
return succ;
}
void Mediator::mediate(){
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);
......@@ -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();
}
std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>> dict;
ros::Rate loop_rate(10);
Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
for(int j = 0; j <= 7; j++){
std::bitset<3> wing_config(j);
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]);
for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear();
rviz_markers_->markers.clear();
ceti1->rotate(0.0872665f);
ceti1->notify();
bool temp = check_collision(0);
/*
if (check_collision(0)) {
approximation(ceti1, j, dict);
approximation(ceti1, j);
} else {
continue;
}
*/
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) pub_->publish(*markers);
pub_->publish(*rviz_markers_);
}
}
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, 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();
void Mediator::approximation(Robot* robot, int& wings){
ROS_INFO("assigne result to first robot...");
Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
std::vector<std::pair<tf2::Transform, int>> second_robot_data;
for (int i = 0; i <= 7; i++){
std::bitset<3> wing_config(i);
build_wings(wing_config, ceti);
for (Abstract_robot_element* fields : robot->access_fields()) {
ceti->set_tf(fields->world_tf());
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->notify();
if (robot->check_robot_collision(ceti)) continue;
if (check_collision(1)) {
ROS_INFO("should work");
second_robot_data.push_back(std::pair<tf2::Transform, int>(ceti->tf(), i));
write_file(robot, wings, ceti, i);
} else {
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()));
}
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;
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()));
}
ROS_INFO("writing items for robot2...");
for(tf2::Vector3& vec : robot_2){
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()));;
}
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);
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();
node[A->name()]["q_x"] = A->tf().getRotation().getX();
node[A->name()]["q_y"] = A->tf().getRotation().getY();
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);
node[A->name()]["wings"] = a;
m.setRotation(B->tf().getRotation());
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.close();
}
void Mediator::build_wings(std::bitset<3>& wing, Robot* robot){
std::bitset<3> result = robot->observer_mask() & wing;
for (int i = 0; i < result.size(); i++){
ROS_INFO( "%i", i);
for (std::size_t i = 0; i < result.size(); 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]);
robot->register_observers(moveit_right);
......
......@@ -8,7 +8,7 @@ void Moveit_mediator::publish_tables(){
moveit_msgs::PlanningScene planning_scene;
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]);
for (moveit_msgs::CollisionObject* markers : ceti->coll_markers()) {
planning_scene.world.collision_objects.push_back(*markers);
......@@ -32,7 +32,7 @@ void Moveit_mediator::mediate(){};
void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){
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]){
moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject();
robot->add_coll_markers(marker);
......
......@@ -42,18 +42,14 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){
if ((std::floor(sum*100)/100.f) <= full_area) {return false; }
for (int i = 0; i < bounds_.size(); i++){
tf2::Transform A = tf_ * bounds_[0];
tf2::Transform B = tf_ * bounds_[1];
tf2::Transform C = tf_ * bounds_[2];
tf2::Transform D = tf_ * bounds_[3];
A = tf_ * bounds_[0];
B = tf_ * bounds_[1];
C = tf_ * bounds_[2];
D = tf_ * bounds_[3];
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);
if ((std::floor(sum*100)/100.f) <= full_area) return true;
}
if (!observers_.empty()){
std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
......@@ -81,14 +77,77 @@ bool Robot::check_single_object_collision(tf2::Transform& obj){
void Robot::reset(){
observers_.clear();
access_fields_.clear();
generate_access_fields();
tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
}
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){
......@@ -99,7 +158,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
int index = 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)){
found = true;
index = i;
......@@ -108,7 +167,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
}
if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
} 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)){
found = true;
index = i;
......@@ -117,7 +176,7 @@ void Robot::register_observers(Abstract_robot_element* wd){
}
if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
} 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){
found = true;
index = i;
......
......@@ -34,4 +34,7 @@ void Wing_moveit_decorator::output_filter() {
markers_->primitive_poses[0].orientation.w = world_quat.getW();
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() {
bounds.color.a = 1.0;
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];
geometry_msgs::Point point;
point.x = point_posistion.getOrigin().getX();
......
#include "../include/mtc2taskspace.h"
int main(int argc, char **argv){
ros::init(argc, argv, "mtc2taskspace");
ros::NodeHandle n;
......@@ -58,7 +60,7 @@ int main(int argc, char **argv){
// lets make some result data
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;
ROS_INFO("writing items for robot1...");
......@@ -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()));;
}
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;
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