void StrColRoSeService::execute() {
mBBThread.reset(new boost::thread(boost::bind(&StrColRoSeService::blackboardReadRun, this)));
Planes detectedPlanes;
Cylinders detectedCylinders;
while (isRunning()&&ros::ok()) {
RoSe::Mutex::AutoLocker lock(mMutex);
mCondition.wait();
if (mLastPointCloud && isRunning() && ros::ok()) {
detectedPlanes.clear();
detectedCylinders.clear();
//segment pointCloud mLastPointCloud
PlanePtrs pointMapping(mLastPointCloud->points.size());
CylinderPtrs cylinderMapping;
mStrCol.doSegmentation(mLastPointCloud, pointMapping, detectedPlanes, cylinderMapping, detectedCylinders);
//send unsegmented points
RosePointcloudPtr unsegmentedPoints(new RosePointcloud);
extractUnmappedPointsToRosePC(unsegmentedPoints, mLastPointCloud, pointMapping);
sendPoints(unsegmentedPoints);
std::cout << unsegmentedPoints->points().size() << " points were not segmented and sent back" << std::endl;
//send detected planes
sendPlanes(detectedPlanes);
mLastPointCloud.reset();
}
}
mBBThread->join();
}
~Impl()
{
// Ensure planes are cleared first (subsectors may include mappings).
planes.clear();
delete [] self()._lookupPlanes;
}