bool mitk::ToFCameraPMDCamBoardController::SetLensCalibration( bool on )
{
if (on)
{
this->m_PMDRes = pmdProcessingCommand(m_PMDHandle, 0, 0, "SetLensCalibration On");
return ErrorText(this->m_PMDRes);
}
else
{
this->m_PMDRes = pmdProcessingCommand(m_PMDHandle, 0, 0, "SetLensCalibration Off");
return ErrorText(this->m_PMDRes);
}
}
bool mitk::ToFCameraPMDCamBoardController::SetFieldOfView( float fov )
{
std::stringstream commandStream;
commandStream<<"SetFOV "<<fov;
this->m_PMDRes = pmdProcessingCommand(m_PMDHandle, 0, 0, commandStream.str().c_str());
return ErrorText(this->m_PMDRes);
}
/**
*
* @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;
}
