/* collects a cloud by aggregating k successive frames */
void waitForCloudK(int k){
ros::Rate r(30);
cloud_aggregated->clear();
int counter = 0;
while (ros::ok()){
ros::spinOnce();
r.sleep();
if (new_cloud_available_flag){
*cloud_aggregated+=*cloud;
new_cloud_available_flag = false;
counter ++;
if (counter >= k){
cloud_aggregated->header = cloud->header;
break;
}
}
}
}
void computeNeonVoxels(PointCloudT::Ptr in, PointCloudT::Ptr green, PointCloudT::Ptr orange) {
green->clear();
orange->clear();
//Point Cloud to store out neon cap
//PointCloudT::Ptr temp_neon_cloud (new PointCloudT);
for (int i = 0; i < in->points.size(); i++) {
unsigned int r, g, b;
r = in->points[i].r;
g = in->points[i].g;
b = in->points[i].b;
// Look for mostly neon value points
if (g > 175 && (r + b) < 150) {
green->push_back(in->points[i]);
}
else if(r > 200 && (g + b) < 150){
orange->push_back(in->points[i]);
}
}
}