simple_base.cpp
Go to the documentation of this file.
1 #include "bridge/simple_base.h"
3 
4 SimpleBase::SimpleBase(std::shared_ptr<ros::NodeHandle> const& d)
5  : AbstractBase(d)
6  , task_space_reader_(std::make_unique<TSReader>(d))
7  , map_reader_(std::make_unique<MapReader>(d))
8  {
9  map_= map_reader_->mapData();
10  ROS_INFO("=> RM is known");
11 
12  for (auto& s_vod:task_space_reader_->dropOffData()){
13  std::vector<std::pair<object_data, std::vector<tf2::Quaternion>>> vod_q;
14  for(auto& od: s_vod.second) vod_q.push_back(std::pair<object_data, std::vector<tf2::Quaternion>>(od, std::vector<tf2::Quaternion>()));
15  task_space_.insert(std::pair< const std::string, std::vector<std::pair<object_data, std::vector<tf2::Quaternion>>>>(s_vod.first,vod_q));
16  }
17  ROS_INFO("=> TS is known");
18  }
19 
21 
22  implementation_->setGraspOrientations(task_space_);
23  ROS_INFO("=> grasp rotations from implenentation object set...");
24 
25 
26  implementation_->invMapCreation(map_, inv_map_, task_space_);
27  ROS_INFO("=> inverse map is set...");
28 
30  ROS_INFO("=> clouds are formed...");
31 
32 
33  ROS_INFO("init voxel...");
34  std::vector<pcl::PointXYZ> voxelization = this->createPCLBox();
35 
36  std::map<const std::string, std::vector<std::vector<int>>> base_target_map;
37  for (auto& ts: task_space_){
38  base_target_map.insert(std::pair<const std::string, std::vector<std::vector<int>>>(ts.first, std::vector<std::vector<int>>(voxelization.size())));
39  }
40 
41  ROS_INFO("start cloud quantization...");
42  for (auto& s_vc: target_cloud_){
43  for(long unsigned int j = 0; j < target_cloud_.at(s_vc.first).size();j++){
44  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f);
45  octree.setInputCloud(target_cloud_.at(s_vc.first)[j]);
46  octree.addPointsFromInputCloud();
47  double min_x, min_y, min_z, max_x, max_y, max_z;
48  octree.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
49  for(long unsigned int k = 0; k < voxelization.size(); k++) {
50  pcl::PointXYZ p = voxelization[k];
51  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);
52  if(isInBox && octree.isVoxelOccupiedAtPoint(p)) {
53  std::vector< int > pointIdxVec;
54  if(octree.voxelSearch(p, pointIdxVec)) if(!pointIdxVec.empty()) base_target_map.at(s_vc.first)[k].push_back(j);
55  }
56  }
57  }
58  }
59 
60  for(auto& s_bv: base_target_map){
61  std::vector<pcl::PointXYZ> points_per_robot;
62  for(int j = 0; j < base_target_map.at(s_bv.first).size(); j++){
63  if (base_target_map.at(s_bv.first)[j].size() == task_space_.at(s_bv.first).size()) {
64  points_per_robot.push_back(voxelization[j]);
65  }
66  }
67  if (!points_per_robot.empty()) result_.insert(std::pair<const std::string, std::vector<pcl::PointXYZ>>(s_bv.first, points_per_robot));
68  }
69 
70 
71 
72  for (auto& s_r: result_) {
73  ROS_INFO("%s got %li base positions to ckeck", s_r.first.c_str(), result_.at(s_r.first).size());
74  }
75 }
76 
77 
78 std::vector<pcl::PointXYZ> AbstractBase::createPCLBox(){
79  tf2::Vector3 origin(0,0,0);
80  float resolution = 0.4f;
81  float diameter = 3.0f;
82  unsigned char depth = 16;
83  std::vector<pcl::PointXYZ> box;
84  octomap::OcTree* tree = new octomap::OcTree(resolution/2);
85  for (float x = origin.getX() - diameter * 5 ; x <= origin.getX() + diameter * 5 ; x += resolution){
86  for (float y = origin.getY() - diameter * 5 ; y <= origin.getY() + diameter * 5 ; y += resolution){
87  for (float z = origin.getZ() - diameter * 1.5 ; z <= origin.getZ() + diameter * 1.5 ; z += resolution){
88  octomap::point3d point(x,y,z);
89  tree->updateNode(point, true);
90  }
91  }
92  }
93 
94  for (octomap::OcTree::leaf_iterator it = tree->begin_leafs(depth), end = tree->end_leafs(); it != end; ++it){
95  pcl::PointXYZ searchPoint(it.getCoordinate().x(), it.getCoordinate().y(), it.getCoordinate().z());
96  box.push_back(searchPoint);
97  }
98 
99  return box;
100 }
TSReader
TS reader.
Definition: ts_reader.h:15
AbstractBase::implementation_
std::shared_ptr< AbstractBaseImplementation > implementation_
refined implementation
Definition: abstract_base.h:37
SimpleBase::map_reader_
std::unique_ptr< MapReader > map_reader_
Definition: simple_base.h:27
SimpleBase::SimpleBase
SimpleBase(std::shared_ptr< ros::NodeHandle > const &d)
Simple base constructor.
Definition: simple_base.cpp:4
MapReader
Map reader.
Definition: map_reader.h:15
AbstractBase::task_space_
std::map< const std::string, std::vector< std::pair< object_data, std::vector< tf2::Quaternion > > > > task_space_
Drop-off locations with their grasp orientations, mapped to a robot.
Definition: abstract_base.h:32
SimpleBase::baseCalculation
void baseCalculation() override
refined Template methode
Definition: simple_base.cpp:20
AbstractBase
AbstractBaseClass.
Definition: abstract_base.h:27
AbstractBase::inv_map_
std::vector< tf2::Transform > inv_map_
InverseReachabilityMap structure (Zacharias)
Definition: abstract_base.h:31
object_data
Object data.
Definition: abstract_param_reader.h:14
AbstractBase::target_cloud_
std::map< const std::string, std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr > > target_cloud_
Pointcloud structure, mapped to robot.
Definition: abstract_base.h:33
AbstractBase::createPCLBox
static std::vector< pcl::PointXYZ > createPCLBox()
createPCLBox box
Definition: simple_base.cpp:78
SimpleBase::task_space_reader_
std::unique_ptr< TSReader > task_space_reader_
Definition: simple_base.h:26
AbstractBase::result_
std::map< const std::string, std::vector< pcl::PointXYZ > > result_
Result basepositions, mapped to robot.
Definition: abstract_base.h:34
simple_base.h
AbstractBase::map_
std::vector< tf2::Transform > map_
ReachabilityMap structure (Zacharias)
Definition: abstract_base.h:30
abstract_base_implementation.h


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