6 , task_space_reader_(std::make_unique<
TSReader>(d))
7 , map_reader_(std::make_unique<
MapReader>(d))
10 ROS_INFO(
"=> RM is known");
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));
17 ROS_INFO(
"=> TS is known");
23 ROS_INFO(
"=> grasp rotations from implenentation object set...");
27 ROS_INFO(
"=> inverse map is set...");
30 ROS_INFO(
"=> clouds are formed...");
33 ROS_INFO(
"init voxel...");
34 std::vector<pcl::PointXYZ> voxelization = this->
createPCLBox();
36 std::map<const std::string, std::vector<std::vector<int>>> base_target_map;
38 base_target_map.insert(std::pair<
const std::string, std::vector<std::vector<int>>>(ts.first, std::vector<std::vector<int>>(voxelization.size())));
41 ROS_INFO(
"start cloud quantization...");
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);
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);
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]);
67 if (!points_per_robot.empty())
result_.insert(std::pair<
const std::string, std::vector<pcl::PointXYZ>>(s_bv.first, points_per_robot));
73 ROS_INFO(
"%s got %li base positions to ckeck", s_r.first.c_str(),
result_.at(s_r.first).size());
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);
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);