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());
21 ROS_INFO(
"caculated [inv_map] contains %li entrys...", var->
inv_map().size());
25 ROS_INFO(
"initialyze thread implementations...");
28 std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_clouds;
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 >));
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++) {
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()));
51 ROS_INFO(
"[cloud]calculation done...");