コード例 #1
0
  bool ToFCameraPMDO3Controller::OpenCameraConnection()
  {
    if(!m_ConnectionCheck)
    {
      m_PMDRes = pmdOpen (&m_PMDHandle , m_SourcePlugin , m_SourceParam , m_ProcPlugin , m_ProcParam );
      m_ConnectionCheck = ErrorText(m_PMDRes);

      // get image properties from camera
      this->UpdateCamera();
      m_PMDRes = pmdGetSourceDataDescription(m_PMDHandle, &m_DataDescription);
      ErrorText(m_PMDRes);
      m_CaptureWidth = m_DataDescription.img.numColumns;
      m_CaptureHeight = m_DataDescription.img.numRows;
      m_PixelNumber = m_CaptureWidth*m_CaptureHeight;
      m_NumberOfBytes = m_PixelNumber * sizeof(float);
      m_SourceDataSize = m_DataDescription.size;
      m_SourceDataStructSize = m_DataDescription.size + sizeof(PMDDataDescription);
      MITK_INFO << "Datasource size: " << m_SourceDataSize <<std::endl;
      MITK_INFO << "Integration Time: " << this->GetIntegrationTime();
      MITK_INFO << "Modulation Frequence: " << this->GetModulationFrequency();

      return m_ConnectionCheck;
    }
    else return m_ConnectionCheck;
  }
コード例 #2
0
int o3d3xx_sample_imager_settings_function(void)
{
	PMDHandle hnd = 0;
    std::string command = "";
	int res = 0;
    char err[256] = {0};
    char response[SOURCE_CMD_BUFFER_LENGTH] = {0};
	unsigned int integrationTime = 0;

    // connect to camera
    printf ("\n ====================O3D300 Sample Code for Imager Settings====================");
    printf ("\n Connecting to camera: \n");
    res = pmdOpen (&hnd, SOURCE_PLUGIN, SOURCE_PARAM, PROC_PLUGIN, PROC_PARAM);

    if (res != PMD_OK)
    {
        fprintf (stderr, "Could not connect: \n");
        getchar();
        return res;
    }

	// Change Exposure Mode to "moderate" 
    printf ("\n -----------------------------------------------------------------------------");
	printf ("\n--------------------Imager Settings Functionality-----------------------------");
    printf ("\n -----------------------------------------------------------------------------");
	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Get Imager Setting Type\n");
	command = "GetImagerSettingType";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not change exposure mode:\n%s\n", err);
		printf (" Press any key to continue....");
        getchar();
    }
	else
	{
		printf("Type: %s", response);
		printf ("\n DONE");
	}
	
	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Set Exposure Mode to 'moderate'\n");
	command = "SetExposureMode 1";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not change exposure mode:\n%s\n", err);
		printf (" Press any key to continue....");
        getchar();
    }
	else
		printf ("\n DONE");

    printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Set unambiguous distance range to 'upto30m'\n");
	command = "SetUnambiguousDistanceRange 1";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not change unambiguous distance range:\n%s\n", err);
		printf (" Press any key to continue....");
        getchar();
    }
	else
		printf ("\n DONE");

	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Get Imager Setting Type\n");
	command = "GetImagerSettingType";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not change exposure mode:\n%s\n", err);
		printf (" Press any key to continue....");
        getchar();
    }
	else
	{
		printf("Type: %s", response);
		printf ("\n DONE");
	}
	
	// Get the current Exposure Time and Exposure Time Ratio 
	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Get Integration Time 1\n");
	// Call pmdGetIntegrationTime with index 0 to get exposure time
	res = pmdGetIntegrationTime (hnd, &integrationTime , 0);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not get integration time 1:\n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
		return res;
    }
	else
	{
		printf("Integration time 1 = %d usec", integrationTime);
		printf ("\n DONE");
	}

	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Get Integration Time Ratio\n");
	command = "GetExposureTimeRatio";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not get integration time ratio:\n%s\n", err);
		getchar();
    }
	else
	{
        int integrationTimeRatio = atoi( response ); 
		printf("Integration time Ratio = %d", integrationTimeRatio);
		printf ("\n DONE");
	}

	// Change Exposure Time and Exposure Time Ratio 
	printf ("\n -----------------------------------------------------------------------------");
    printf ("\n Setting exposureTime to 1000 usec \n");
	// Call pmdSetIntegrationTime with index 0 to set exposure time
    res = pmdSetIntegrationTime (hnd, 0 , 1000);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, "Could not set the exposure time: \n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
		return res;
    }
	else
		printf("\n DONE");

	printf ("\n -----------------------------------------------------------------------------");
    printf ("\n Setting exposureTimeRatio to 50 \n");
	command = "SetExposureTimeRatio 50";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, "Could not set the exposure time ratio: \n%s\n", err);
		getchar();
    }
	else
		printf("\n DONE");

    printf ("\n -----------------------------------------------------------------------------");
    printf ("\n Getting Exposure Time List \n");
	command = "GetExposureTimeList";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, "Could not get the exposure time list: \n%s\n", err);
		getchar();
    }
	else
    {
        printf("Exposure Time List = %s", response);
		printf("\n DONE");
    }

	// Optimizing exposure time : Setting integration time 10,000 usec before optimizing 
	// exposure time to see the effect of optimizing functionality
	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Set Exposure Mode to 'low'\n");
	command = "SetExposureMode 0";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not change exposure mode:\n%s\n", err);
		printf (" Press any key to continue....");
        getchar();
    }
	else
		printf ("\n DONE");

	printf ("\n -----------------------------------------------------------------------------");
    printf ("\n Setting integrationTime to 10,000 usec \n");

    res = pmdSetIntegrationTime (hnd, 0 , 10000);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, "Could not set the integration time: \n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
		return res;
    }

	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Call Optimize function\n");
	command = "OptimizeExposureTime";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Error in Optimization Procedure:\n%s\n", err);
		printf (" Press any key to continue....");
        getchar();
    }
	else
		printf ("\n DONE");

	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Get Integration Time 1\n");
	res = pmdGetIntegrationTime (hnd, &integrationTime , 0);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not get integration time 1:\n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
		return res;
    }
	else
	{
		printf("Integration time 1 = %d usec", integrationTime);
		printf ("\n DONE");
	}

	unsigned int modulationFrequencyChannel;
	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Get Modulation Frequency Channel\n");
	res = pmdGetModulationFrequency (hnd, &modulationFrequencyChannel, 0);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not get modulation frequency:\n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
		return res;
    }
	else
	{
		printf("Modulation Frequency Channel = %d", modulationFrequencyChannel);
		printf ("\n DONE");
	}

	modulationFrequencyChannel = 2;
	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Set Modulation Frequency Channel to 2\n");
	res = pmdSetModulationFrequency (hnd, 0, modulationFrequencyChannel);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not set modulation frequency:\n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
		return res;
    }
	else
		printf ("\n DONE");


	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Get Modulation Frequency Channel\n");
	res = pmdGetModulationFrequency (hnd, &modulationFrequencyChannel, 0);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not get modulation frequency:\n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
		return res;
    }
	else
	{
		printf("Modulation Frequency Channel = %d", modulationFrequencyChannel);
		printf ("\n DONE");
	}

	modulationFrequencyChannel = 1;
	printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Set Modulation Frequency Channel to 2\n");
	res = pmdSetModulationFrequency (hnd, 0, modulationFrequencyChannel);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not set modulation frequency:\n%s\n", err);
		pmdClose (hnd);
		printf("Camera Connection Closed. \n");
		printf ("Exiting application after key press....");
        getchar();
        return res;
    }
	else
		printf ("\n DONE");

    printf ("\n -----------------------------------------------------------------------------");
	printf ("\n---------------Source Commands: Invalid Imager Settings Parameters------------");
    printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Setting Exposure Mode with invalid arguments: ""SetExposureMode 5""\n");
	command = "SetExposureMode 5";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        printf (" Result: \n");
		pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not set exposure mode:\n%s\n", err);
    }
	else
		printf ("\n DONE");

    printf ("\n -----------------------------------------------------------------------------");
	printf ("\n Setting Unambiguous Range with invalid arguments: ""SetUnambiguousDistanceRange 5""\n");
	command = "SetUnambiguousDistanceRange 5";
	res = pmdSourceCommand (hnd, response, SOURCE_CMD_BUFFER_LENGTH, command.c_str());
    if (res != PMD_OK)
    {
        printf (" Result: \n");
		pmdGetLastError (hnd, err, 128);
        fprintf (stderr, " Could not set unambiguous distance range:\n%s\n", err);
    }
	else
		printf ("\n DONE");


    // Closing the connection.
    res = pmdClose (hnd);
    if (res != PMD_OK)
    {
        pmdGetLastError (hnd, err, 128);
        fprintf (stderr, "Could not close the connection %s\n", err);

        return res;
    }
    printf ("\n Camera closed Successfully");
    printf ("\n ================================================================================");


    return PMD_OK;
}
コード例 #3
0
/**
 *
 * @brief Initialize the camera and initial parameter values. Returns 1 if properly initialized.
 *
 * @param [in] int
 * @param [in] argv
 * @param [in] ros::NodeHandle
 *
 */
int initialize(int argc, char *argv[],ros::NodeHandle nh){
	/*
	 * Inital Setup for parameters
	 */
	integrationTime = 1500;
	modulationFrequency = 30000000;
	frameRate = 40;
	bilateralFilter = false;
	
	flip_x = flip_y = 1;
	
	AmplitudeFilterOn = false;
	AmplitudeThreshold = 0;

	for( int i = 1; i < argc; i++) {
		// reading width
		if( std::string(argv[i]) == "-it" ) {
			if( sscanf(argv[++i], "%d", &integrationTime) != 1 
				|| integrationTime < 100 || integrationTime > 2700 ) {
				ROS_WARN("*invalid integration time");
				return help();
			}
		}
		// reading heigth
		else if( std::string(argv[i]) == "-mf" ) {
			if( sscanf(argv[++i], "%d", &modulationFrequency) != 1 
				|| modulationFrequency < 5000000 || modulationFrequency > 30000000 ) {
				ROS_WARN("*invalid modulation frequency");
				return help();
			}
		}
		if( std::string(argv[i]) == "-fr" ) {
			if( sscanf(argv[++i], "%d", &frameRate) != 1 
				|| frameRate < 1 || frameRate > 40 ) {
				ROS_WARN("*invalid frame rate");
				return help();
			}
		} else if( std::string(argv[i]) == "-bf" ) {
			bilateralFilter = true;
		} else if( std::string(argv[i]) == "-flip_x" ) {
			flip_x = -1;
		} else if( std::string(argv[i]) == "-flip_y" ) {
			flip_y = -1;
		}
		// additional parameters
		else if( std::string(argv[i]) == "-af" ) {
			AmplitudeFilterOn = true;
		}
		else if( std::string(argv[i]) == "-at" ) {
			if( sscanf(argv[++i], "%f", &AmplitudeThreshold) != 1 
				|| AmplitudeThreshold < 0 || AmplitudeThreshold > 2500 ) {
				ROS_WARN("*invalid amplitude threshold");
				return help();
			}	
		}
		// print help
		else if( std::string(argv[i]) == "--help" ) {
			ROS_WARN_STREAM("arguments: " << argc << " which: " << argv[i]);		
			return help();
		}
		else if( argv[i][0] == '-' ) {
			ROS_WARN_STREAM("invalid option " << argv[i]);
			return help();
		}
	} 	

	/*
	 * Camera Initialization
	 */
	std::stringstream sourcePluginLocation, procPluginLocation;
	sourcePluginLocation.clear();
	procPluginLocation.clear();
	sourcePluginLocation << PMD_PLUGIN_DIR << "digicam";
	procPluginLocation << PMD_PLUGIN_DIR << "digicamproc";

	// If the camera is not connected at all, we will get an segmentation fault.
	res = pmdOpen (&hnd, sourcePluginLocation.str().c_str(), SOURCE_PARAM, procPluginLocation.str().c_str(), PROC_PARAM);
	if (res != PMD_OK)
	{
		pmdGetLastError (0, err, 128);
		ROS_ERROR_STREAM("Could not connect: " << err);
		return 0;
	}

	char result[128];
	result[0] = 0;
	res = pmdSourceCommand(hnd, result, sizeof(result), "IsCalibrationDataLoaded");
	if (res != PMD_OK)
	{
		pmdGetLastError (0, err, 128);
		ROS_ERROR_STREAM("Could not execute source command: " << err);
		pmdClose (hnd);
		return 0;
	}
	if (std::string(result) == "YES")
		ROS_INFO("Calibration file loaded.");
	else
		ROS_INFO("No calibration file found");

	res = pmdUpdate (hnd);
	if (res != PMD_OK)
	{
		pmdGetLastError (hnd, err, 128);
		ROS_ERROR_STREAM("Could not transfer data: " << err);
		pmdClose (hnd);
		return 0;
	}

	PMDDataDescription dd;

	res = pmdGetSourceDataDescription (hnd, &dd);
	if (res != PMD_OK)
	{
		pmdGetLastError (hnd, err, 128);
		ROS_ERROR_STREAM("Could not get data description: " << err);
		pmdClose (hnd);
		return 0;
	}

	if (dd.subHeaderType != PMD_IMAGE_DATA)
	{
		ROS_ERROR_STREAM("Source data is not an image!\n");
		pmdClose (hnd);
		return 0;
	}
	noOfRows = dd.img.numRows;
	noOfColumns = dd.img.numColumns;

	/*
	 * ROS Node Initialization
	 */
	pub_non_filtered = nh.advertise<PointCloud> ("depth_non_filtered", 1);
	pub_filtered = nh.advertise<PointCloud> ("depth_filtered", 1);
	dataPublished=true;
	return 1;
}