simple_base_implementation.cpp
Go to the documentation of this file.
2 
3 void Simple_base_implementation::set_grasp_orientations(std::map<const std::string, std::vector<std::pair<object_data, std::vector<tf2::Quaternion>>>>& var){
4  tf2::Quaternion x_rot(0,0,0,1);
5  tf2::Quaternion y_rot(0,0,0.707,0.707);
6  tf2::Quaternion z_rot(0,-0.707,0,0.707);
7  std::vector<tf2::Quaternion> basic_rot{x_rot, x_rot.inverse(), y_rot, y_rot.inverse(), z_rot.inverse()};
8  ROS_INFO("=> 5 Basic grasp roatations (x, -x, y, -y, -z) ");
9  for(auto&s_od_q: var){
10  for(auto&od_q: s_od_q.second){
11  od_q.second = basic_rot;
12  }
13  }
14 }
15 
16 void Simple_base_implementation::inv_map_creation(std::vector<tf2::Transform>& map, std::vector<tf2::Transform>& inv_map, std::map<const std::string, std::vector<std::pair<object_data,std::vector<tf2::Quaternion>>>>& task_space){
17  std::vector<tf2::Transform> trans;
18  for(long unsigned int i = 0; i < map.size(); i++) {
19  for(auto&s_od_q: task_space){
20  for(auto&od_q: s_od_q.second){
21  for(auto&q: od_q.second){
22  if (q.angle(map[i].getRotation()) < 0.349066f){
23  inv_map.push_back(tf2::Transform(q, map[i].getOrigin()).inverse());
24  break;
25  }
26  }
27  }
28  }
29  }
30 
31  ROS_INFO("caculated [inv_map] contains %li entrys...", inv_map.size());
32 }
33 
34 /*
35 void Simple_base_implementation::cloud_calculation(Abstract_base* var){
36  ROS_INFO("initialyze thread implementations...");
37 
38  for(auto& s_od_q: var->task_space()) {
39  std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr> clouds;
40  for(auto& od_q: s_od_q.second) {
41  clouds.push_back(pcl::PointCloud< pcl::PointXYZ >::Ptr(new pcl::PointCloud< pcl::PointXYZ >));
42  }
43  var->target_cloud().insert(std::pair<const std::string, std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>(s_od_q.first, clouds));
44  }
45 
46  // Maybe OpenMP
47  ROS_INFO("start [cloud] calculation...");
48  // ------------------------------------------------------------
49  tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
50  // ------------------------------------------------------------
51  for(auto& s_c: var->target_cloud()){
52  for(int i = 0; i < s_c.second.size();i++){
53  for(auto& itf : var->inv_map()){
54  for(auto& r: var->task_space().at(s_c.first)[i].second){
55  tf2::Transform target(r, var->task_space().at(s_c.first)[i].first.pose_.getOrigin());
56  tf2::Transform base = target * (itf * root);
57  s_c.second[i]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
58  }
59  }
60  }
61  }
62 
63  ROS_INFO("[cloud]calculation done...");
64 
65 }
66 */
object_data
Definition: abstract_param_reader.h:13
Simple_base_implementation::inv_map_creation
void inv_map_creation(std::vector< tf2::Transform > &map, std::vector< tf2::Transform > &inv_map, std::map< const std::string, std::vector< std::pair< object_data, std::vector< tf2::Quaternion >>>> &task_space) override
Create inversed reachability map.
Definition: simple_base_implementation.cpp:16
Simple_base_implementation::set_grasp_orientations
void set_grasp_orientations(std::map< const std::string, std::vector< std::pair< object_data, std::vector< tf2::Quaternion >>>> &var) override
Store grasp orientations as endeffector transforms.
Definition: simple_base_implementation.cpp:3
simple_base_implementation.h


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