/**
*
* @brief Publish the data based on set up parameters.
*
*/
int publishData() {
/*
* Update Camera settings
*/
res = pmdUpdate (hnd);
if (res != PMD_OK)
{
pmdGetLastError (hnd, err, 128);
ROS_ERROR_STREAM("Could transfer data: " << err);
pmdClose (hnd);
return 0;
}
/*
* Obtain PointClouds
*/
if (!cartesianDist)
cartesianDist = new float [noOfRows * noOfColumns * 3];
res = pmdGet3DCoordinates (hnd, cartesianDist, noOfColumns * noOfRows * 3 * sizeof (float));
if (res != PMD_OK)
{
pmdGetLastError (hnd, err, 128);
ROS_ERROR_STREAM("Could get cartesian coordinates: " << err);
pmdClose (hnd);
return 0;
}
/*
* Obtain Amplitude Values
*/
if (!amplitudes)
amplitudes = new float [noOfRows * noOfColumns];
res = pmdGetAmplitudes (hnd, amplitudes, noOfRows * noOfColumns * sizeof (float));
if (res != PMD_OK)
{
pmdGetLastError (hnd, err, 128);
ROS_ERROR_STREAM("Could get amplitude values: " << err);
pmdClose (hnd);
return 1;
}
/*
* Creating the pointcloud
*/
// Fill in the cloud data
PointCloud::Ptr msg_non_filtered (new PointCloud);
msg_non_filtered->header.frame_id = "tf_argos3d";
msg_non_filtered->height = 1;
msg_non_filtered->width = noOfRows*noOfColumns;
PointCloud::Ptr msg_filtered (new PointCloud);
msg_filtered->header.frame_id = "tf_argos3d";
msg_filtered->width = 1;
msg_filtered->height = noOfColumns*noOfRows;
msg_filtered->is_dense = false;
//msg_filtered->points.resize (noOfRows*noOfColumns);
int countWidth=0;
for (size_t i = 0; i < noOfRows*noOfColumns; ++i) {
pcl::PointXYZI temp_point;
temp_point.x = cartesianDist[(i*3) + 0]*flip_x;
temp_point.y = cartesianDist[(i*3) + 1]*flip_y;
temp_point.z = cartesianDist[(i*3) + 2];
temp_point.intensity = amplitudes[i];
if(AmplitudeFilterOn==true && amplitudes[i]>AmplitudeThreshold) {
msg_filtered->points.push_back(temp_point);
countWidth++;
}
msg_non_filtered->points.push_back(temp_point);
}
msg_filtered->height = countWidth;
/*
* Publishing the messages
*/
if(AmplitudeFilterOn){
#if ROS_VERSION > ROS_VERSION_COMBINED(1,9,49)
msg_filtered->header.stamp = ros::Time::now().toNSec();
#else
msg_filtered->header.stamp = ros::Time::now();
#endif
pub_filtered.publish (msg_filtered);
}
#if ROS_VERSION > ROS_VERSION_COMBINED(1,9,49)
msg_non_filtered->header.stamp = ros::Time::now().toNSec();
#else
msg_non_filtered->header.stamp = ros::Time::now();
#endif
pub_non_filtered.publish (msg_non_filtered);
return 1;
}
bool ToFCameraPMDController::UpdateCamera()
{
m_PMDRes = pmdUpdate(m_PMDHandle);
return ErrorText(m_PMDRes);
}
/**
*
* @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;
}
