base_by_rotation.cpp
Go to the documentation of this file.
2 
3 
5  ROS_INFO("starting base calculation implementation...");
6  ROS_INFO("condition based [inv_map] calculation...");
7  std::vector<tf2::Transform> trans;
8  for(long unsigned int i = 0; i < var->map().size(); i++) {
9  for (long unsigned int x = 0; x < var->target_rot().size(); x++) {
10  for (long unsigned int y = 0; y < var->target_rot()[x].size(); y++) {
11  for(long unsigned int p = 0; p < var->target_rot()[x][y].size(); p++){
12  if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.349066f) {
13  trans.push_back(tf2::Transform(var->target_rot()[x][y][p], var->map()[i].getOrigin()).inverse());
14  break;
15  }
16  }
17  }
18  }
19  }
20  var->set_inv_map(trans);
21  ROS_INFO("caculated [inv_map] contains %li entrys...", var->inv_map().size());
22 };
23 
25  ROS_INFO("initialyze thread implementations...");
26 
27  // Optimize targets in later implementation
28  std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_clouds;
29  target_clouds.resize(var->task_grasps().size());
30  for(long unsigned int i = 0; i < target_clouds.size(); i++) {
31  for (long unsigned int j = 0; j < var->task_grasps()[i].size();j++) {
32  target_clouds[i].push_back(pcl::PointCloud< pcl::PointXYZ >::Ptr(new pcl::PointCloud< pcl::PointXYZ >));
33  }
34  }
35 
36  // Maybe OpenMP
37  ROS_INFO("start [cloud] calculation...");
38  tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
39  for (long unsigned int i = 0; i < target_clouds.size(); i++){
40  for (long unsigned int j = 0; j < target_clouds[i].size(); j++) {
41  for (long unsigned int y = 0; y < var->inv_map().size(); y++) {
42  for (long unsigned int x = 0; x < var->target_rot()[i][j].size(); x++) {
43  tf2::Transform target = var->task_grasps()[i][j]; target.setRotation(var->target_rot()[i][j][x]);
44  tf2::Transform base = target * (var->inv_map()[y] * root);
45  target_clouds[i][j]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
46  }
47  }
48  }
49  }
50  var->set_target_cloud(target_clouds);
51  ROS_INFO("[cloud]calculation done...");
52 }
Abstract_map_loader::set_inv_map
void set_inv_map(std::vector< tf2::Transform > &list)
Definition: abstract_map_loader.h:43
Abstract_map_loader::inv_map
std::vector< tf2::Transform > & inv_map()
Definition: abstract_map_loader.h:41
Base_by_rotation::inv_map_creation
void inv_map_creation(Abstract_map_loader *var) override
Definition: base_by_rotation.cpp:4
Abstract_map_loader::map
std::vector< tf2::Transform > & map()
Definition: abstract_map_loader.h:42
Abstract_map_loader::set_target_cloud
void set_target_cloud(std::vector< std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr >> &cloud)
Definition: abstract_map_loader.h:49
Abstract_map_loader::task_grasps
std::vector< std::vector< tf2::Transform > > & task_grasps()
Definition: abstract_map_loader.h:46
Base_by_rotation::cloud_calculation
void cloud_calculation(Abstract_map_loader *var) override
Definition: base_by_rotation.cpp:24
Abstract_map_loader
Definition: abstract_map_loader.h:24
base_by_rotation.h
Abstract_map_loader::target_rot
std::vector< std::vector< std::vector< tf2::Quaternion > > > & target_rot()
Definition: abstract_map_loader.h:47


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43