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;
}
}