Ejemplo n.º 1
0
void BiclopsAccess::biclopsUpdateLoop()
{
	bool updated = false;

	//cout << "Firing up Biclops update thread." << endl;
// Publish Position & Speed
     
      
	while(!_threadKill)
	{

		for(int jnt = 0; jnt < BICLOPS_NUM_JOINTS; jnt++)
		{
			if(joint_commands[jnt] != joint_last_set[jnt])
			{
				pthread_mutex_lock(&position_mutex);
				double val = joint_commands[jnt];
				joint_last_set[jnt] = val;
				updated = true;
				pthread_mutex_unlock(&position_mutex);

				double unit = radiansToUnit(val, jnt);
	
				PMDAxisControl::Profile p;
				PMDAxisControl *axis = _biclops->GetAxis(jnt);

				axis->SetPosition(axis->UnitsToCounts(unit));
				axis->GetProfile(p);
				p.pos = unit;
				axis->SetProfile(p);

				axis->Move(false, true);
			}

	       
		}

	
		if(updated)
		{
			usleep(BICLOPS_UPDATE_SLEEP_TIME_MS*1000);
			updated = false;
		}
		else
		{
			usleep(0);
		}
	};
}
Ejemplo n.º 2
0
int main() {
	Biclops biclops;

	// Create a debug header message for use in this example application.
	const char dbgMethod[] = "main: ";
	char dbgHdr[32];
	sprintf(dbgHdr,"BiclopsPMDTest::main:");

	// Defines which axes we want to use.
	const int axisMask = Biclops::PanMask
					   + Biclops::TiltMask;

	// Pointers to each axis (populated once controller is initialized).
	PMDAxisControl *panAxis = NULL;
	PMDAxisControl *tiltAxis = NULL;

	// Set Debug level depending on how much info you want to see about
	// the inner workings of the API. Level 2 is highest with 0 being
	// the default (i.e., no messages).
	biclops.SetDebugLevel(2);

	//setting up the Kinect
	Mat rgbMat(Size(640,480),CV_8UC3);
	Mat depthMat(Size(640,480),CV_16UC1);
	//specify path to calibration file
	char calib_filepath[] = "calib/kinect_calibration2.yml";
	//initialize the kinect
	kinect myKinect(0, calib_filepath, false);
	myKinect.start();

	coutDbg << "Initializing Biclops\n";
	if (biclops.Initialize("data/BiclopsDefault.cfg")) {

		cout << "Initializing Kinect" << endl;
		for(int i=0;i<10;i++)
		{
			myKinect.getVideo(rgbMat);
			myKinect.getDepth(depthMat);
		}

		// Initialize the Biclops unit.
		coutDbg << "Initializing Biclops\n";
		// Initialization completed successfully.
		coutDbg << "Biclops initialized\n";
		biclops.SetDebugLevel(0);

		// Get shortcut references to each axis.
		panAxis = biclops.GetAxis(Biclops::Pan);
		tiltAxis = biclops.GetAxis(Biclops::Tilt);

		coutDbg << "Begin homing sequence." << endl;
		if (biclops.HomeAxes(axisMask,true)) {

			// Get the currently defined (default) motion profiles.
			PMDAxisControl::Profile panProfile,tiltProfile;
			panAxis->GetProfile(panProfile);
			tiltAxis->GetProfile(tiltProfile);

			// Set a position to move to by modifying the respective profiles.
			// NOTE: profile values are in revolutions, so here we convert
			// from degrees (divide by 360) for readability.
			panProfile.pos = PMDUtils::DegsToRevs(panAngles[0]);
			tiltProfile.pos = PMDUtils::DegsToRevs(tiltAngles[0]);

			// Inform the controller of the new desired position.
			panAxis->SetProfile(panProfile);
			tiltAxis->SetProfile(tiltProfile);

			// Move one axis to its destination in series.
			coutDbg << "Individual axis move" << endl;
			panAxis->Move();
			tiltAxis->Move();
			coutDbg << "Individual axis move complete" << endl;

			//create folder to store the Kinect data
			int fCount = 0;
			string mainFolder = "Scan";
			std::ostringstream folderPath;
			for(;;)
			{
				folderPath.str("");
				folderPath << mainFolder << fCount;
				if(!utils::checkDirectory(folderPath.str(), false))
				{
					utils::checkDirectory(folderPath.str(), true);
					break;
				}
				fCount++;
			}

			for(int i=0;i<sizeof(tiltAngles)/sizeof(double);i++)
			{
				for(int j=0;j<sizeof(panAngles)/sizeof(double);j++)
				{

					// Pick a new destination and tell the controller.
					panProfile.pos = PMDUtils::DegsToRevs(panAngles[j]);
					panAxis->SetProfile(panProfile);
					tiltProfile.pos = PMDUtils::DegsToRevs(tiltAngles[i]);
					tiltAxis->SetProfile(tiltProfile);

					coutDbg << "Moving to Pan: " << panAngles[j] << " Tilt: " << tiltAngles[i] << endl;
					// Issue the move command for all the axes simultaneously.
					coutDbg << "Multiple axis move" << endl;
					biclops.Move(axisMask);
					coutDbg << "Multiple axis move complete" << endl;
					if(j==0)
						sleep(4);
					else
						sleep(2);

					//creating folders and subfolders to store the images
					std::ostringstream subFolderPath, rgbFolderPath, depthFolderPath;
					subFolderPath << folderPath.str() << "/P" << panAngles[j] << "T" << tiltAngles[i];
					rgbFolderPath << subFolderPath.str() << "/rgb";
					depthFolderPath << subFolderPath.str() << "/depth";
					utils::checkDirectory(subFolderPath.str(), true);
					utils::checkDirectory(rgbFolderPath.str(), true);
					utils::checkDirectory(depthFolderPath.str(), true);

					//grabbing kinect frames
					for(int k=0;k<10;k++)
					{
						myKinect.getVideo(rgbMat);
						myKinect.getDepth(depthMat);

						//save kinect frames to folder
						std::ostringstream rgbPath, depthPath;
						rgbPath << rgbFolderPath.str() << "/rgb" << k << ".png";
						depthPath << depthFolderPath.str() << "/depth" << k << ".png";
						cv::imwrite(rgbPath.str().c_str(),rgbMat);
						cv::imwrite(depthPath.str().c_str(),depthMat);
					}
				}
			}

			//back to home position
			panProfile.pos = PMDUtils::DegsToRevs(0);
			panAxis->SetProfile(panProfile);
			tiltProfile.pos = PMDUtils::DegsToRevs(0);
			tiltAxis->SetProfile(tiltProfile);
			biclops.Move(axisMask);

			// Turn off motor amps to conserve power.
			panAxis->DisableAmp();
			tiltAxis->DisableAmp();
		}
		else {
			coutDbg << "initialization failed" << endl;
		}
	}

	// Stop the Kinect
	myKinect.stop();

	return 0;
}