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();
}
Ejemplo n.º 2
0
//--------------------------------------------------------------
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();
}