int DataProcessor::get_object_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_orig, pcl::PointCloud<pcl::PointXYZRGB>::Ptr planar_cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); //Filtered cloud pcl::PointCloud<pcl::PointXYZRGB>::Ptr everything_else(new pcl::PointCloud<pcl::PointXYZRGB>); /****************** Filter out the non-table points ******************/ pass_through_gen(pcd_orig,pcd_filtered,true,0,1.0,true,-0.6,0.6,true,0,0.8); //pcl::io::savePCDFileASCII ("scene3.pcd", *pcd_filtered); /* ********* Segmentation of the cloud into table and object clouds **********/ string str1 = "table_try.pcd", str2 = "everything_else.pcd"; planar_seg(pcd_filtered,planar_cloud,everything_else,str1.c_str(), str2.c_str()); //planar_seg(pcd_filtered,planar_cloud,everything_else,"table_try.pcd","everything_else.pcd"); pubCloud("planar_cloud", planar_cloud, "base_link"); /*********** Find dimensions of table **********/ std::vector<geometry_msgs::Point> extent = find_extents(*planar_cloud); table_extent_ = extent; /********** Now filter the object cloud based on table dimensions *********/ pass_through_gen(everything_else,object_cloud,true,extent[0].x, extent[1].x-0.05, true,extent[2].y+0.01,extent[3].y-0.005,true,extent[5].z-0.01,extent[5].z+0.15); pubCloud("object_cloud", object_cloud, "base_link"); //pcl::io::savePCDFileASCII ("object_try.pcd", *object_cloud); return (0); }
void DataProcessor::process() { vector<pcl::PointCloud<pcl::PointXYZRGB> > region_cloud; //vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr > region_ptr; vector<geometry_msgs::Point> object_extent = find_extents(*object_cloud_); marker_pub_.publish(set_boundaries("base_link","obj_extent",1, visualization_msgs::Marker::LINE_STRIP,visualization_msgs::Marker::ADD, object_extent,0.005,1.0f,0.0f,0.0f,1.0)); //Pick up every block duplo_v1::Manipulate_Duplo grasp_call; vector<sensor_msgs::PointCloud2> bricks = cluster(object_cloud_, 0); ROS_INFO("Need to pick up %zu bricks", bricks.size()); if (bricks.size() == 0) { nocluster_count_++; new_cloud_wanted_ = true; } else { nocluster_count_ = 0; pick_n_place(bricks[0]); new_cloud_wanted_ = true; } if (nocluster_count_ == 3) { ROS_INFO("Table cleared!"); ros::shutdown(); } if (new_cloud_wanted_) { ROS_INFO("DUPLO: Asking for new point cloud."); //ros::Duration(2).sleep(); duplo_v1::Get_New_PCD srv_newpcd; srv_newpcd.request.question = true; if (client_newpcd_.call(srv_newpcd)) ROS_INFO("DUPLO: Requesting for new point cloud."); else { ROS_ERROR("Failed to call service get new pcd."); return; } new_cloud_wanted_ = false; } }
//---------------------------------------------------------------------------// //recursively build an AABB tree from the set of triangles static void build_aabb_tree ( LTAABB_Node& nd, //parent node LTAABB_Node nodes[],//node array uint16& nc, //nodes count uint16 ti[], //sorted triangle indices const LTVector3f& min, //parent extents const LTVector3f& max, const uint16 it[], //unsorted triangle indices const uint16 tc, //triangle index count const LTTriangle tri[], //triangle array const LTVector3f V[] //vertex array ) { uint16 lc;//number of triangles on the "left" side of the box sort_triangles( lc, ti, min, max, it, tc, tri, V ); //find out which child the unshared //extents correspond to LTVector3f lmin, lmax, rmin, rmax; nd.Init();//initialize the node find_extents( nd, lmin, lmax, min, max, ti, lc, tri, V ); nd.F = ~nd.F;//flip bits so that true's become false's find_extents( nd, rmin, rmax, min, max, ti + lc, tc - lc, tri, V ); nd.F = ~nd.F;//restore bits //recalculate floating point extent //values after truncation nd.Min( lmin, rmin, min, max ); nd.Max( lmax, rmax, min, max ); if( 1 == lc )//terminate { nd.F |= LTAABB_Node::L_LEAF;//set bit nd.L = ti[0];//first in list } else//recurse { nd.F &= ~LTAABB_Node::L_LEAF;//unset bit //recurse on kids (switch ti[] and it[]) build_aabb_tree(nodes[nd.L = nc++], //left child nodes, nc, (uint16*)it, lmin, lmax, ti, lc, tri, V ); } if( tc - 1 == lc )//terminate { nd.F |= LTAABB_Node::R_LEAF;//set bit nd.R = ti[lc]; } else { nd.F &= ~LTAABB_Node::R_LEAF;//unset bit //(NOTE: since 'nc' gets incremented in the //above call, must set nd.R down here build_aabb_tree(nodes[nd.R = nc++],//right child nodes, nc, (uint16*)(it + lc), rmin, rmax, ti + lc, tc - lc, tri, V ); } }
static void finish_pattern(char *line) { /* end of pattern layout */ char symmetry; /* the symmetry character */ int max_assistance = 0; /* maximum value from assistance */ mini = minj = 0; /* initially : can change with edge-constraints */ if (num_stars > 1 || (pattern_type == PATTERNS && num_stars == 0)) { fprintf(stderr, "No or too many *'s in pattern %s\n", pattern_names[patno]); fatal_errors=1; } if (ci == -1 || cj == -1) { fprintf(stderr, "No origin for pattern %s\n", pattern_names[patno]); fatal_errors=1; ci=0; cj=0; } /* translate posn of * to relative co-ords */ if (num_stars == 1) { pattern[patno].movei -= ci; pattern[patno].movej -= cj; } else if (num_stars == 0) { pattern[patno].movei = -1; pattern[patno].movej = -1; } find_extents(); compute_grids(); /* now read in the weights of the line * It is not necessary for all the values to be given, * but at least two fields must be supplied. * The compiler guarantees that all the fields are already * initialised to 0. */ pattern[patno].patlen = el; { int s; char klass[32]; char *p = line; int n; klass[0] = 0; /* in case sscanf doesn't get that far */ s = sscanf(p, ":%c%n", &symmetry, &n); if (pattern_type == PATTERNS && s == 1) { p += n; s += sscanf(p, ",%d,%n", &pattern[patno].patwt, &n); if (s == 2) p += n; pattern[patno].assistance = NO_ASSISTANCE; if (strncmp(p,"wind",4) == 0) { int ucutoff; int uvalue; int mycutoff; int myvalue; pattern[patno].assistance = WIND_ASSISTANCE; p += 4; sscanf(p, "(%d,%d,%d,%d),%n", &ucutoff, &uvalue, &mycutoff, &myvalue, &n); p += n; printf("static int assist_params%d[] = {%d,%d,%d,%d};\n\n", patno, ucutoff, uvalue, mycutoff, myvalue); max_assistance = ucutoff*uvalue + mycutoff*myvalue; } else if (strncmp(p,"moyo",4) == 0) { int moyocutoff; int moyovalue; pattern[patno].assistance = MOYO_ASSISTANCE; p += 4; sscanf(p, "(%d,%d),%n", &moyocutoff, &moyovalue, &n); p += n; printf("static int assist_params%d[] = {%d,%d};\n\n", patno, moyocutoff, moyovalue); max_assistance = moyocutoff * moyovalue; } else { sscanf(p, "%*[^,],%n", &n); p += n; } s += sscanf(p, "%[^,],%d,%d,%d,%d,%d,%s", klass, &pattern[patno].obonus, &pattern[patno].xbonus, &pattern[patno].splitbonus, &pattern[patno].minrand, &pattern[patno].maxrand, helper_fn_names[patno]); } else if (pattern_type == CONNECTIONS && s==1) { p += n; s += sscanf(p, ",%[^,],%s", klass, helper_fn_names[patno]); } { if (strchr(klass,'s')) pattern[patno].klass |= CLASS_s; if (strchr(klass,'O')) pattern[patno].klass |= CLASS_O; if (strchr(klass,'o')) pattern[patno].klass |= CLASS_o; if (strchr(klass,'X')) pattern[patno].klass |= CLASS_X; if (strchr(klass,'x')) pattern[patno].klass |= CLASS_x; if (strchr(klass,'D')) pattern[patno].klass |= CLASS_D; if (strchr(klass,'C')) pattern[patno].klass |= CLASS_C; if (strchr(klass,'n')) pattern[patno].klass |= CLASS_n; if (strchr(klass,'B')) pattern[patno].klass |= CLASS_B; if (strchr(klass,'A')) pattern[patno].klass |= CLASS_A; if (strchr(klass,'L')) pattern[patno].klass |= CLASS_L; } switch (pattern_type) { case PATTERNS: if (s < 2) { fprintf(stderr, ": line must contain weight\n"); ++fatal_errors; } break; case HALFEYES: if (s != 1) { fprintf(stderr, ": line must contain only symmetry in -h mode\n"); ++fatal_errors; } break; case CONNECTIONS: if (s < 2) { fprintf(stderr, ": line must contain class in -c mode\n"); ++fatal_errors; } break; } } /* Now get the symmetry. There are extra checks we can make to do with * square-ness and edges. We do this before we work out the edge constraints, * since that mangles the size info. */ switch(symmetry) { case '+' : if (where & (NORTH|EAST|SOUTH|WEST)) fprintf(stderr, "Warning : symmetry inconsistent with edge constraints (pattern %s)\n", pattern_names[patno]); pattern[patno].trfno = 2; break; case 'X' : if (where & (NORTH|EAST|SOUTH|WEST)) fprintf(stderr, "Warning : X symmetry inconsistent with edge constraints (pattern %s)\n", pattern_names[patno]); if (maxi != maxj) fprintf(stderr, "Warning : X symmetry requires a square pattern (pattern %s)\n", pattern_names[patno]); pattern[patno].trfno = 2; break; case '-' : if (where & (NORTH|SOUTH)) fprintf(stderr, "Warning : symmetry inconsistent with edge constraints (pattern %s)\n", pattern_names[patno]); pattern[patno].trfno = 4; break; case '|' : if (where & (EAST|WEST)) fprintf(stderr, "Warning : symmetry inconsistent with edge constraints (pattern %s)\n", pattern_names[patno]); pattern[patno].trfno = 4; break; case '\\' : case '/' : /* FIXME : can't be bothered putting in the edge tests */ if (maxi != maxj) fprintf(stderr, "Warning : \\ or / symmetry requires a square pattern (pattern %s)\n", pattern_names[patno]); pattern[patno].trfno = 4; break; default: fprintf(stderr, "Warning : symmetry character '%c' not implemented - using '8' (pattern %s)\n", symmetry, pattern_names[patno]); /* FALLTHROUGH */ case '8' : pattern[patno].trfno = 8; break; } if (pattern_type == PATTERNS) { pattern[patno].maxwt = pattern[patno].patwt + max_assistance + pattern[patno].splitbonus + pattern[patno].maxrand; if (pattern[patno].obonus > 0) pattern[patno].maxwt += pattern[patno].obonus; if (pattern[patno].xbonus > 0) pattern[patno].maxwt += pattern[patno].xbonus; } }