simple_base_implementation.cpp
Go to the documentation of this file.
2 
3 void SimpleBaseImplementation::setGraspOrientations(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 SimpleBaseImplementation::invMapCreation(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 SimpleBaseImplementation::cloudCalculation(std::vector<tf2::Transform>& inv_map, std::map<const std::string, std::vector<std::pair<object_data,std::vector<tf2::Quaternion>>>>& task_space, std::map<const std::string, std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& target_cloud){
36  ROS_INFO("initialyze thread implementations...");
37 
38  for(auto& s_od_q: 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  target_cloud.insert(std::pair<const std::string, std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>(s_od_q.first, clouds));
44  }
45 
46  ROS_INFO("start [cloud] calculation...");
47  // ------------------------------------------------------------
48  tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
49  // ------------------------------------------------------------
50  for(auto& s_c: target_cloud){
51  for(int i = 0; i < s_c.second.size();i++){
52  for(auto& itf : inv_map){
53  for(auto& r: task_space.at(s_c.first)[i].second){
54  tf2::Transform target(r, task_space.at(s_c.first)[i].first.pose_.getOrigin());
55  tf2::Transform base = target * (itf * root);
56  s_c.second[i]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
57  }
58  }
59  }
60  }
61 
62  ROS_INFO("[cloud]calculation done...");
63 
64 }
object_data
Object data.
Definition: abstract_param_reader.h:14
SimpleBaseImplementation::cloudCalculation
void cloudCalculation(std::vector< tf2::Transform > &inv_map, std::map< const std::string, std::vector< std::pair< object_data, std::vector< tf2::Quaternion >>>> &task_space, std::map< const std::string, std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr >> &target_cloud) override
PCL Cloud calculation for every Object.
Definition: simple_base_implementation.cpp:35
SimpleBaseImplementation::setGraspOrientations
void setGraspOrientations(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
SimpleBaseImplementation::invMapCreation
void invMapCreation(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.h


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51