bool mitk::ToFCameraPMDCamBoardController::SetFPNCalibration( bool on )
{
if(on)
{
this->m_PMDRes=pmdSourceCommand(m_PMDHandle,0,0,"SetFPNCalibration On");
return this->ErrorText(this->m_PMDRes);
}
else
{
this->m_PMDRes=pmdSourceCommand(m_PMDHandle,0,0,"SetFPNCalibration Off");
return this->ErrorText(this->m_PMDRes);
}
}
float mitk::ToFCameraPMDCamBoardController::GetDistanceOffset()
{
char offset[16];
this->m_PMDRes = pmdSourceCommand(m_PMDHandle, offset, 16, "GetSoftOffset");
ErrorText(this->m_PMDRes);
return atof(offset);
}
bool mitk::ToFCameraPMDCamBoardController::SetDistanceOffset( float offset )
{
std::stringstream command;
command<<"SetSoftOffset "<<offset;
this->m_PMDRes = pmdSourceCommand(m_PMDHandle,0,0,command.str().c_str());
return ErrorText(this->m_PMDRes);
}
bool mitk::ToFCameraPMDCamBoardController::SetExposureMode( int mode )
{
if (mode==0) // normal mode
{
this->m_PMDRes = pmdSourceCommand(m_PMDHandle, 0, 0, "SetExposureMode Normal");
return ErrorText(this->m_PMDRes);
}
else if (mode==1) // SMB mode
{
this->m_PMDRes = pmdSourceCommand(m_PMDHandle, 0, 0, "SetExposureMode SMB");
return ErrorText(this->m_PMDRes);
}
else
{
MITK_ERROR<<"Specified exposure mode not supported. Exposure mode must be 0 (Normal) or 1 (SMB)";
return false;
}
}
bool mitk::ToFCameraPMDCamBoardController::SetRegionOfInterest( unsigned int leftUpperCornerX, unsigned int leftUpperCornerY, unsigned int width, unsigned int height )
{
// check if leftUpperCornerX and width are divisible by 3 otherwise round to the next value divisible by 3
unsigned int factor = leftUpperCornerX/3;
leftUpperCornerX = 3*factor;
factor = width/3;
width = 3*factor;
std::stringstream command;
command<<"SetROI "<<leftUpperCornerX<<" "<<leftUpperCornerY<<" "<<width<<" "<<height;
this->m_PMDRes = pmdSourceCommand(m_PMDHandle,0,0,command.str().c_str());
return ErrorText(this->m_PMDRes);
}
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;
}
/**
*
* @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;
}
/**
*
* @brief Callback for rqt_reconfigure. It is called any time we change a
* parameter in the visual interface
*
* @param [in] argos3d_p100::argos3d_p100Config
* @param [in] uint32_t
*
*/
void callback(argos3d_p100::argos3d_p100Config &config, uint32_t level)
{
// Check the configuretion parameters with those given in the initialization
if(first) {
config.Integration_Time = integrationTime;
config.Modulation_Frequency = modulationFrequency;
config.Frame_rate = frameRate;
config.Bilateral_Filter = !bilateralFilter;
if (flip_x == -1)
config.Flip_X = true;
if (flip_y == -1)
config.Flip_Y = true;
config.Amplitude_Filter_On = AmplitudeFilterOn;
config.Amplitude_Threshold = AmplitudeThreshold;
integrationTime = modulationFrequency = frameRate = -1;
}
if(integrationTime != config.Integration_Time) {
integrationTime = config.Integration_Time;
res = pmdSetIntegrationTime (hnd, 0, integrationTime);
if (res != PMD_OK)
{
pmdGetLastError (hnd, err, 128);
ROS_WARN_STREAM("Could not set integration time: " << err);
}
}
if(modulationFrequency != config.Modulation_Frequency) {
modulationFrequency = config.Modulation_Frequency;
res = pmdSetModulationFrequency(hnd, 0, modulationFrequency);
if (res != PMD_OK) {
pmdGetLastError (hnd, err, 128);
ROS_WARN_STREAM("Could not set modulation frequency: " << err);
}
}
if(frameRate != config.Frame_rate) {
frameRate = config.Frame_rate;
err[0] = 0;
std::stringstream spt;
spt << "SetPhaseTime " << (1000000/(4*frameRate));
res = pmdSourceCommand (hnd, err, sizeof(err), spt.str().c_str());
if (res != PMD_OK) {
pmdGetLastError (hnd, err, 128);
ROS_WARN_STREAM("Could not set frame rate: " << err);
}
}
if(bilateralFilter != config.Bilateral_Filter) {
bilateralFilter = config.Bilateral_Filter;
err[0] = 0;
if(bilateralFilter)
res = pmdProcessingCommand(hnd, err, sizeof(err), "SetBilateralFilter on");
else
res = pmdProcessingCommand(hnd, err, sizeof(err), "SetBilateralFilter off");
if (res != PMD_OK) {
pmdGetLastError (hnd, err, 128);
ROS_WARN_STREAM("Could not set bilateral filter: " << err);
}
}
flip_x = flip_y = 1;
if (config.Flip_X)
flip_x = -1;
if (config.Flip_Y)
flip_y = -1;
AmplitudeFilterOn = config.Amplitude_Filter_On;
AmplitudeThreshold = config.Amplitude_Threshold;
}
