basler_tof_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2017, DFKI GmbH
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of DFKI GmbH nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *      Author:
00030  *         Martin Günther <martin.guenther@dfki.de>
00031  */
00032 
00033 #include <ConsumerImplHelper/ToFCamera.h>
00034 #include <GenTL/PFNC.h>
00035 #include <string>
00036 #include <iostream>
00037 #include <fstream>
00038 #include <iomanip>
00039 
00040 #include <ros/ros.h>
00041 #include <camera_info_manager/camera_info_manager.h>
00042 #include <sensor_msgs/Image.h>
00043 #include <sensor_msgs/CameraInfo.h>
00044 #include <sensor_msgs/image_encodings.h>
00045 #include <cv_bridge/cv_bridge.h>
00046 #include <opencv2/opencv.hpp>
00047 #include <pcl_ros/point_cloud.h>
00048 #include <pcl_conversions/pcl_conversions.h>
00049 
00050 #include <dynamic_reconfigure/server.h>
00051 #include <basler_tof/BaslerToFConfig.h>
00052 
00053 using namespace GenTLConsumerImplHelper;
00054 using namespace GenApi;
00055 using namespace std;
00056 
00057 ros::Publisher cloud_pub_;
00058 ros::Publisher intensity_pub_;
00059 ros::Publisher intensity_ci_pub_;
00060 ros::Publisher confidence_pub_;
00061 ros::Publisher confidence_ci_pub_;
00062 ros::Publisher depth_pub_;
00063 ros::Publisher depth_ci_pub_;
00064 std::string frame_id_;
00065 std::string device_id_;
00066 std::string camera_name_;
00067 CToFCamera camera_;
00068 
00069 bool subscriber_connected_;
00070 
00071 boost::shared_ptr<camera_info_manager::CameraInfoManager> intensity_info_manager_;
00072 boost::shared_ptr<camera_info_manager::CameraInfoManager> confidence_info_manager_;
00073 boost::shared_ptr<camera_info_manager::CameraInfoManager> depth_info_manager_;
00074 
00075 
00076 bool publish(const BufferParts& parts, ros::Time acquisition_time)
00077 {
00078   if (parts.size() != 3)
00079   {
00080     ROS_ERROR("Expected 3 parts, got %zu!", parts.size());
00081     return false;
00082   }
00083 
00084   // If the point cloud is enabled, the first part always contains the point cloud data.
00085   if (parts[0].dataFormat != PFNC_Coord3D_ABC32f)
00086   {
00087     ROS_ERROR("Unexpected data format for the first image part. Coord3D_ABC32f is expected.");
00088     return false;
00089   }
00090 
00091   if (parts[1].dataFormat != PFNC_Mono16)
00092   {
00093     ROS_ERROR("Unexpected data format for the second image part. Mono16 is expected.");
00094     return false;
00095   }
00096 
00097   // ----- publish point cloud
00098   const size_t nPixel = parts[0].width * parts[0].height;
00099 
00100   pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>());
00101   cloud->header.frame_id = frame_id_;
00102   cloud->header.stamp = pcl_conversions::toPCL(acquisition_time);
00103   cloud->width = parts[0].width;
00104   cloud->height = parts[0].height;
00105   cloud->is_dense = false;
00106   cloud->points.resize(nPixel);
00107 
00108   CToFCamera::Coord3D *pPoint = static_cast<CToFCamera::Coord3D*>(parts[0].pData);
00109   uint16_t *pIntensity = static_cast<uint16_t*>(parts[1].pData);
00110 
00111   for (size_t i = 0; i < nPixel; ++i)
00112   {
00113     pcl::PointXYZI &p = cloud->points[i];
00114     if (pPoint->IsValid())
00115     {
00116       p.x = 0.001f * pPoint->x;
00117       p.y = 0.001f * pPoint->y;
00118       p.z = 0.001f * pPoint->z;
00119       p.intensity = *pIntensity;
00120     }
00121     else
00122     {
00123       p.x = std::numeric_limits<float>::quiet_NaN();
00124       p.y = std::numeric_limits<float>::quiet_NaN();
00125       p.z = std::numeric_limits<float>::quiet_NaN();
00126     }
00127     pPoint++;
00128     pIntensity++;
00129   }
00130 
00131   cloud_pub_.publish(cloud);
00132 
00133   // ----- publish intensity image
00134   pIntensity = static_cast<uint16_t*>(parts[1].pData);
00135 
00136   cv_bridge::CvImage intensity_cvimg;
00137   intensity_cvimg.encoding = sensor_msgs::image_encodings::MONO16;
00138   intensity_cvimg.header.frame_id = frame_id_;
00139   intensity_cvimg.header.stamp = acquisition_time;
00140   intensity_cvimg.image = cv::Mat(parts[1].height, parts[1].width, CV_16UC1);
00141   intensity_cvimg.image.setTo(0);
00142 
00143   for (size_t i = 0; i < parts[1].height; i++)
00144   {
00145     for (size_t j = 0; j < parts[1].width; j++)
00146     {
00147       intensity_cvimg.image.at<unsigned short>(i, j) = static_cast<unsigned short>(*pIntensity);
00148       pIntensity++;
00149     }
00150   }
00151   // uncomment these two lines for cameracalibrator.py
00152   // intensity_cvimg.image.convertTo(intensity_cvimg.image, CV_8U, 1.0 / 256.0);
00153   // intensity_cvimg.encoding = sensor_msgs::image_encodings::MONO8;
00154 
00155   intensity_pub_.publish(intensity_cvimg.toImageMsg());
00156 
00157   sensor_msgs::CameraInfoPtr intensity_info_msg(new sensor_msgs::CameraInfo(intensity_info_manager_->getCameraInfo()));
00158   intensity_info_msg->header.stamp    = acquisition_time;
00159   intensity_info_msg->header.frame_id = frame_id_;
00160   intensity_ci_pub_.publish(intensity_info_msg);
00161 
00162   // ----- publish confidence image
00163   uint16_t *pConfidence = static_cast<uint16_t*>(parts[2].pData);
00164 
00165   cv_bridge::CvImage confidence_cvimg;
00166   confidence_cvimg.encoding = sensor_msgs::image_encodings::MONO16;
00167   confidence_cvimg.header.frame_id = frame_id_;
00168   confidence_cvimg.header.stamp = acquisition_time;
00169   confidence_cvimg.image = cv::Mat(parts[2].height, parts[2].width, CV_16UC1);
00170   confidence_cvimg.image.setTo(0);
00171 
00172   for (size_t i = 0; i < parts[2].height; i++)
00173   {
00174     for (size_t j = 0; j < parts[2].width; j++)
00175     {
00176       confidence_cvimg.image.at<unsigned short>(i, j) = static_cast<unsigned short>(*pConfidence);
00177       pConfidence++;
00178     }
00179   }
00180 
00181   confidence_pub_.publish(confidence_cvimg.toImageMsg());
00182 
00183   sensor_msgs::CameraInfoPtr confidence_info_msg(new sensor_msgs::CameraInfo(confidence_info_manager_->getCameraInfo()));
00184   confidence_info_msg->header.stamp    = acquisition_time;
00185   confidence_info_msg->header.frame_id = frame_id_;
00186   confidence_ci_pub_.publish(confidence_info_msg);
00187 
00188   // ----- publish depth image
00189   pPoint = static_cast<CToFCamera::Coord3D*>(parts[0].pData);
00190 
00191   cv_bridge::CvImage depth_cvimg;
00192   depth_cvimg.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00193   depth_cvimg.header.frame_id = frame_id_;
00194   depth_cvimg.header.stamp = acquisition_time;
00195   depth_cvimg.image = cv::Mat(parts[0].height, parts[0].width, CV_16UC1);
00196   depth_cvimg.image.setTo(0);
00197 
00198   for (size_t i = 0; i < parts[0].height; i++)
00199   {
00200     for (size_t j = 0; j < parts[0].width; j++)
00201     {
00202       depth_cvimg.image.at<uint16_t>(i, j) = static_cast<uint16_t>(pPoint->z);   // can be NaN; should be in mm
00203       pPoint++;
00204     }
00205   }
00206 
00207   depth_pub_.publish(depth_cvimg.toImageMsg());
00208 
00209   sensor_msgs::CameraInfoPtr depth_info_msg(new sensor_msgs::CameraInfo(depth_info_manager_->getCameraInfo()));
00210   depth_info_msg->header.stamp    = acquisition_time;
00211   depth_info_msg->header.frame_id = frame_id_;
00212   depth_ci_pub_.publish(depth_info_msg);
00213 
00214   return true;
00215 }
00216 
00217 void round_to_increment_int(int &param, int increment)
00218 {
00219   param = ((param + increment / 2) / increment) * increment;
00220 }
00221 
00222 void round_to_increment_double(double &param, double increment)
00223 {
00224   param = (int)((param + increment / 2) / increment) * increment;
00225 }
00226 
00227 void update_config(basler_tof::BaslerToFConfig &new_config, uint32_t level)
00228 {
00229   // round to increments
00230   round_to_increment_int(new_config.exposure_time, 100);
00231   round_to_increment_double(new_config.exposure_agility, 0.1);
00232   round_to_increment_int(new_config.confidence_threshold, 16);
00233   round_to_increment_int(new_config.temporal_strength, 5);
00234   round_to_increment_int(new_config.outlier_tolerance, 16);
00235 
00236   CFloatPtr(camera_.GetParameter("AcquisitionFrameRate"))->SetValue(new_config.frame_rate);
00237 
00238   if (new_config.exposure_auto)
00239   {
00240     CEnumerationPtr(camera_.GetParameter("ExposureAuto"))->FromString("Continuous");
00241 
00242     // Agility and Delay are only valid when exposure_auto is "true"
00243     CFloatPtr(camera_.GetParameter("Agility"))->SetValue(new_config.exposure_agility);
00244     CIntegerPtr(camera_.GetParameter("Delay"))->SetValue(new_config.exposure_delay);
00245   }
00246   else
00247   {
00248     CEnumerationPtr(camera_.GetParameter("ExposureAuto"))->FromString("Off");
00249 
00250     // ExposureTime is only valid when exposure_auto is "false"
00251     CFloatPtr(camera_.GetParameter("ExposureTime"))->SetValue(new_config.exposure_time);
00252   }
00253 
00254   CIntegerPtr(camera_.GetParameter("ConfidenceThreshold"))->SetValue(new_config.confidence_threshold);
00255   CBooleanPtr(camera_.GetParameter("FilterSpatial"))->SetValue(new_config.spatial_filter);
00256   CBooleanPtr(camera_.GetParameter("FilterTemporal"))->SetValue(new_config.temporal_filter);
00257   CIntegerPtr(camera_.GetParameter("FilterStrength"))->SetValue(new_config.temporal_strength);
00258   CIntegerPtr(camera_.GetParameter("OutlierTolerance"))->SetValue(new_config.outlier_tolerance);
00259 }
00260 
00261 void subscribeCallback()
00262 {
00263   if (cloud_pub_.getNumSubscribers() > 0 ||
00264       intensity_pub_.getNumSubscribers() > 0 ||
00265       confidence_pub_.getNumSubscribers() > 0 ||
00266       depth_pub_.getNumSubscribers() > 0 ||
00267       intensity_ci_pub_.getNumSubscribers() > 0 ||
00268       confidence_ci_pub_.getNumSubscribers() > 0 ||
00269       depth_ci_pub_.getNumSubscribers() > 0)
00270   {
00271     if (!subscriber_connected_)
00272     {
00273       ROS_INFO("Starting stream");
00274       subscriber_connected_ = true;
00275     }
00276   }
00277   else
00278   {
00279     if (subscriber_connected_)
00280     {
00281       ROS_INFO("Stopping stream");
00282       camera_.StopGrab();
00283       subscriber_connected_ = false;
00284     }
00285   }
00286 }
00287 
00288 int main(int argc, char* argv[])
00289 {
00290   ros::init(argc, argv, "basler_tof_node");
00291   ros::NodeHandle n;
00292   ros::NodeHandle pn("~");
00293   ros::NodeHandle intensity_nh("intensity");
00294   ros::NodeHandle confidence_nh("confidence");
00295   ros::NodeHandle depth_nh("depth");
00296 
00297   pn.param("frame_id", frame_id_, std::string("camera_optical_frame"));
00298   if (!pn.getParam("device_id", device_id_))
00299   {
00300     ROS_WARN("~device_id is not set! Using first device.");
00301     device_id_ = "#1";
00302   }
00303 
00304   ros::SubscriberStatusCallback rsscb = boost::bind(&subscribeCallback);
00305 
00306   cloud_pub_ = n.advertise<pcl::PointCloud<pcl::PointXYZI> > ("points", 10, rsscb, rsscb);
00307   intensity_pub_ = n.advertise<sensor_msgs::Image>("intensity/image_raw", 10, rsscb, rsscb);
00308   confidence_pub_ = n.advertise<sensor_msgs::Image>("confidence/image_raw", 10, rsscb, rsscb);
00309   depth_pub_ = n.advertise<sensor_msgs::Image>("depth/image_raw", 10, rsscb, rsscb);
00310   intensity_ci_pub_ = n.advertise<sensor_msgs::CameraInfo>("intensity/camera_info", 10, rsscb, rsscb);
00311   confidence_ci_pub_ = n.advertise<sensor_msgs::CameraInfo>("confidence/camera_info", 10, rsscb, rsscb);
00312   depth_ci_pub_ = n.advertise<sensor_msgs::CameraInfo>("depth/camera_info", 10, rsscb, rsscb);
00313 
00314   intensity_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(intensity_nh);
00315   confidence_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(confidence_nh);
00316   depth_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(depth_nh);
00317 
00318   int exitCode = EXIT_FAILURE;
00319 
00320   try
00321   {
00322     CToFCamera::InitProducer();
00323 
00324     if (device_id_ == "#1")
00325     {
00326       ROS_INFO("Opening first camera.");
00327       camera_.OpenFirstCamera();
00328     }
00329     else
00330     {
00331       CameraList cameraList = camera_.EnumerateCameras();
00332       bool found = false;
00333       for (CameraList::const_iterator it = cameraList.begin(); it != cameraList.end(); ++it)
00334       {
00335         ROS_INFO("Found camera device with serial: %s", it->strSerialNumber.c_str());
00336         if (it->strSerialNumber == device_id_)
00337         {
00338           ROS_INFO("Serial matches device_id, opening camera.");
00339           found = true;
00340           camera_.Open(*it);
00341           break;
00342         }
00343       }
00344       if (!found)
00345       {
00346         ROS_FATAL("No camera with device_id '%s' found, exiting!", device_id_.c_str());
00347         return EXIT_FAILURE;
00348       }
00349     }
00350 
00351     std::string camera_name_ = camera_.GetCameraInfo().strModelName
00352                                + "_" + camera_.GetCameraInfo().strSerialNumber;
00353     if (!intensity_info_manager_->setCameraName(camera_name_) ||
00354         !confidence_info_manager_->setCameraName(camera_name_) ||
00355         !depth_info_manager_->setCameraName(camera_name_))
00356     {
00357       ROS_WARN_STREAM("[" << camera_name_
00358                       << "] name not valid"
00359                       << " for camera_info_manager");
00360     }
00361 
00362     std::string camera_info_url;
00363     if (pn.getParam("camera_info_url", camera_info_url))
00364     {
00365       if (!intensity_info_manager_->validateURL(camera_info_url) ||
00366           !confidence_info_manager_->validateURL(camera_info_url) ||
00367           !depth_info_manager_->validateURL(camera_info_url))
00368       {
00369         ROS_WARN("camera_info_url invalid: %s", camera_info_url.c_str());
00370       }
00371       else
00372       {
00373         intensity_info_manager_->loadCameraInfo(camera_info_url);
00374         confidence_info_manager_->loadCameraInfo(camera_info_url);
00375         depth_info_manager_->loadCameraInfo(camera_info_url);
00376       }
00377     }
00378 
00379     ROS_INFO_STREAM("[" << camera_name_ << "] opened.");
00380 
00381     ROS_INFO_STREAM("DeviceVendorName:      " << CStringPtr(camera_.GetParameter("DeviceVendorName"))->GetValue());
00382     ROS_INFO_STREAM("DeviceModelName:       " << CStringPtr(camera_.GetParameter("DeviceModelName"))->GetValue());
00383     ROS_INFO_STREAM("DeviceVersion:         " << CStringPtr(camera_.GetParameter("DeviceVersion"))->GetValue());
00384     ROS_INFO_STREAM("DeviceFirmwareVersion: " << CStringPtr(camera_.GetParameter("DeviceFirmwareVersion"))->GetValue());
00385     ROS_INFO_STREAM("DeviceDriverVersion:   " << CStringPtr(camera_.GetParameter("DeviceDriverVersion"))->GetValue());
00386     ROS_INFO_STREAM("DeviceSerialNumber:    " << CStringPtr(camera_.GetParameter("DeviceSerialNumber"))->GetValue());
00387 
00388     ROS_INFO_STREAM("DeviceCalibVersion:    " << CIntegerPtr(camera_.GetParameter("DeviceCalibVersion"))->GetValue());
00389     ROS_INFO_STREAM("DeviceCalibState:      " << CEnumerationPtr(camera_.GetParameter("DeviceCalibState"))->ToString());
00390     ROS_INFO_STREAM("DeviceCalibOffset:     " << CIntegerPtr(camera_.GetParameter("DeviceCalibOffset"))->GetValue());
00391 
00392     ROS_INFO_STREAM("DeviceTemperature:     " << CFloatPtr(camera_.GetParameter("DeviceTemperature"))->GetValue() << " degrees C");
00393 
00394 
00395     // Parameterize the camera to send 3D coordinates and intensity data
00396     CEnumerationPtr ptrImageComponentSelector = camera_.GetParameter("ImageComponentSelector");
00397     CBooleanPtr ptrImageComponentEnable = camera_.GetParameter("ImageComponentEnable");
00398     CEnumerationPtr ptrPixelFormat = camera_.GetParameter("PixelFormat");
00399 
00400     ptrImageComponentSelector->FromString("Range");
00401     ptrImageComponentEnable->SetValue(true);
00402     ptrPixelFormat->FromString("Coord3D_ABC32f");
00403 
00404     ptrImageComponentSelector->FromString("Intensity");
00405     ptrImageComponentEnable->SetValue(true);
00406     ptrPixelFormat->FromString("Mono16");
00407 
00408     ptrImageComponentSelector->FromString("Confidence");
00409     ptrImageComponentEnable->SetValue(true);
00410     ptrPixelFormat->FromString("Confidence16");
00411 
00412     dynamic_reconfigure::Server<basler_tof::BaslerToFConfig> dynamic_reconfigure_server;
00413     dynamic_reconfigure::Server<basler_tof::BaslerToFConfig>::CallbackType f;
00414     f = boost::bind(&update_config, _1, _2);
00415     dynamic_reconfigure_server.setCallback(f);
00416 
00417      //check once if subscribers are already connected
00418     subscribeCallback();
00419     while (ros::ok())
00420     {
00421       ros::spinOnce();
00422 
00423       if (subscriber_connected_)
00424       {
00425         // Acquire one single image
00426         BufferParts parts;
00427         GrabResultPtr ptrGrabResult = camera_.GrabSingleImage(1000, &parts);
00428 
00429         // Save 3D data
00430         if (ptrGrabResult->status == GrabResult::Ok)
00431         {
00432           publish(parts, ros::Time::now());
00433         }
00434         else
00435         {
00436             ROS_ERROR("Failed to grab an image.");
00437         }
00438       }
00439     }
00440 
00441     camera_.Close();
00442     exitCode = EXIT_SUCCESS;
00443   }
00444   catch (GenICam::GenericException& e)
00445   {
00446     ROS_ERROR_STREAM("Exception occurred: " << endl << e.GetDescription());
00447   }
00448 
00449   if (CToFCamera::IsProducerInitialized())
00450     CToFCamera::TerminateProducer();  // Won't throw any exceptions
00451 
00452   return exitCode;
00453 }
00454 


basler_tof
Author(s): Martin Guenther
autogenerated on Wed Mar 6 2019 03:36:19