bool ToFCameraPMDController::GetAmplitudes(float* amplitudeArray)
{
float* tempArray = new float[m_CaptureWidth*m_CaptureHeight];
this->m_PMDRes = pmdGetAmplitudes(m_PMDHandle, tempArray, this->m_NumberOfBytes);
TransformCameraOutput(tempArray, amplitudeArray, false);
delete[] tempArray;
return ErrorText(this->m_PMDRes);
}
bool ToFCameraPMDController::GetAmplitudes(float* amplitudeArray)
{
this->m_PMDRes = pmdGetAmplitudes(m_PMDHandle, amplitudeArray, this->m_NumberOfBytes);
return ErrorText(this->m_PMDRes);
}
/**
*
* @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;
}
