/** * * @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; }