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();
}
//--------------------------------------------------------------
void testApp::update(){
#ifdef TARGET_OSX // only working on Mac at the moment
hardware.update();
#endif
// update all nodes
recordContext.update();
recordDepth.update();
// demo getting depth pixels directly from depth gen
depthRangeMask.setFromPixels(recordDepth.getDepthPixels(nearThreshold, farThreshold),
recordDepth.getWidth(), recordDepth.getHeight(), OF_IMAGE_GRAYSCALE);
// update tracking/recording nodes
recordUser.update();
// demo getting pixels from user gen
allUserMasks.setFromPixels(recordUser.getUserPixels(), recordUser.getWidth(), recordUser.getHeight(), OF_IMAGE_GRAYSCALE);
user1Mask.setFromPixels(recordUser.getUserPixels(1), recordUser.getWidth(), recordUser.getHeight(), OF_IMAGE_GRAYSCALE);
ofxTrackedUser* user = recordUser.getTrackedUser(1);
sendPoints(user->neck.position[0], 0);
sendPoints(user->neck.position[1], 1);
sendPoints(user->left_shoulder.position[1], 2);
sendPoints(user->left_upper_arm.position[1], 3);
sendPoints(user->left_lower_arm.position[1], 4);
sendPoints(user->right_shoulder.position[1], 5);
sendPoints(user->right_upper_arm.position[1], 6);
sendPoints(user->right_lower_arm.position[1], 7);
sendPoints(user->left_upper_torso.position[1], 8);
sendPoints(user->left_lower_torso.position[1], 9);
sendPoints(user->left_upper_leg.position[1], 10);
sendPoints(user->left_lower_leg.position[1], 11);
sendPoints(user->right_lower_torso.position[1], 12);
sendPoints(user->right_upper_leg.position[1], 13);
sendPoints(user->right_lower_leg.position[1], 14);
scratch.update();
}
