00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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
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
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
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
00152
00153
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
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
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);
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 ¶m, int increment)
00218 {
00219 param = ((param + increment / 2) / increment) * increment;
00220 }
00221
00222 void round_to_increment_double(double ¶m, 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
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
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
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
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
00418 subscribeCallback();
00419 while (ros::ok())
00420 {
00421 ros::spinOnce();
00422
00423 if (subscriber_connected_)
00424 {
00425
00426 BufferParts parts;
00427 GrabResultPtr ptrGrabResult = camera_.GrabSingleImage(1000, &parts);
00428
00429
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();
00451
00452 return exitCode;
00453 }
00454