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
00043 #include <bta_ros/bta_ros.hpp>
00044
00045 namespace bta_ros
00046 {
00047
00048 BtaRos::BtaRos(ros::NodeHandle nh_camera,
00049 ros::NodeHandle nh_private,
00050 std::string nodeName) :
00051 nh_(nh_camera),
00052 nh_private_(nh_private),
00053 it_(nh_camera),
00054 cim_tof_(nh_camera),
00055 nodeName_(nodeName),
00056 config_init_(false),
00057 _xyz (new sensor_msgs::PointCloud2)
00058 {
00059
00060
00061
00062
00063
00064
00065
00066 transformStamped.header.stamp = ros::Time::now();
00067 transformStamped.header.frame_id = "world";
00068 transformStamped.child_frame_id = "cloud";
00069 transformStamped.transform.translation.x = 0.0;
00070 transformStamped.transform.translation.y = 0.0;
00071 transformStamped.transform.translation.z = 0.0;
00072 tf::Quaternion q;
00073 q.setRPY(0, 0, 0);
00074 transformStamped.transform.rotation.x = q.x();
00075 transformStamped.transform.rotation.y = q.y();
00076 transformStamped.transform.rotation.z = q.z();
00077 transformStamped.transform.rotation.w = q.w();
00078 pub_tf.sendTransform(transformStamped);
00079 }
00080
00081 BtaRos::~BtaRos()
00082 {
00083 ROS_INFO("closing.");
00084 close();
00085 }
00086
00087 void BtaRos::close()
00088 {
00089 ROS_DEBUG("Close called");
00090 if (BTAisConnected(handle_)) {
00091 ROS_DEBUG("Closing..");
00092 BTA_Status status;
00093 status = BTAclose(&handle_);
00094 printf("done: %d \n", status);
00095 }
00096
00097 return;
00098 }
00099
00100
00101 void BtaRos::callback(bta_ros::bta_rosConfig &config_, uint32_t level)
00102 {
00103 BTA_Status status;
00104
00105 int it;
00106 double fr;
00107 uint32_t usValue;
00108
00109 if (!config_init_) {
00110 if (nh_private_.getParam(nodeName_+"/integrationTime",it)) {
00111 usValue = (uint32_t)it;
00112 status = BTAsetIntegrationTime(handle_, usValue);
00113 if (status != BTA_StatusOk)
00114 ROS_WARN_STREAM("Error setting IntegrationTime:: " << status << "---------------");
00115 } else {
00116 status = BTAgetIntegrationTime(handle_, &usValue);
00117 if (status != BTA_StatusOk)
00118 ROS_WARN_STREAM("Error reading IntegrationTime: " << status << "---------------");
00119 else
00120 nh_private_.setParam(nodeName_+"/integrationTime", (int)usValue);
00121 }
00122 nh_private_.getParam(nodeName_+"/integrationTime",config_.Integration_Time);
00123
00124 if (nh_private_.getParam(nodeName_+"/frameRate",fr)) {
00125 status = BTAsetFrameRate(handle_, fr);
00126 if (status != BTA_StatusOk)
00127 ROS_WARN_STREAM("Error setting FrameRate: " << status << "---------------");
00128 } else {
00129 float fr_f;
00130 status = BTAgetFrameRate(handle_, &fr_f);
00131 fr = fr_f;
00132 if (status != BTA_StatusOk)
00133 ROS_WARN_STREAM("Error reading FrameRate: " << status << "---------------");
00134 else
00135 nh_private_.setParam(nodeName_+"/frameRate", fr);
00136 }
00137 nh_private_.getParam(nodeName_+"/frameRate",config_.Frame_rate);
00138 config_init_ = true;
00139 return;
00140 }
00141
00142
00143 nh_private_.getParam(nodeName_+"/integrationTime",it);
00144 if(it != config_.Integration_Time) {
00145 usValue = (uint32_t)config_.Integration_Time;
00146 status = BTAsetIntegrationTime(handle_, usValue);
00147 if (status != BTA_StatusOk)
00148 ROS_WARN_STREAM("Error setting IntegrationTime: " << status << "---------------");
00149 else
00150 nh_private_.setParam(nodeName_+"/integrationTime", config_.Integration_Time);
00151 }
00152
00153 nh_private_.getParam(nodeName_+"/frameRate",fr);
00154 if(fr != config_.Frame_rate) {
00155 usValue = (uint32_t)config_.Frame_rate;
00156 status = BTAsetFrameRate(handle_, usValue);
00157 if (status != BTA_StatusOk)
00158 ROS_WARN_STREAM("Error setting FrameRate: " << status << "---------------");
00159 else
00160 nh_private_.setParam(nodeName_+"/frameRate", config_.Frame_rate);
00161 }
00162
00163 if(config_.Read_reg) {
00164 try {
00165 std::stringstream ss;
00166 it = strtoul(config_.Reg_addr.c_str(), NULL, 0);
00167 status = BTAreadRegister(handle_, it, &usValue, 0);
00168 if (status != BTA_StatusOk) {
00169 ROS_WARN_STREAM("Could not read reg: " << config_.Reg_addr << ". Status: " << status);
00170 }
00171 ss<<"";
00172 ss << "0x"<< std::hex << usValue;
00173 ss >> config_.Reg_val;
00174 ROS_INFO_STREAM("Read register: " << config_.Reg_addr << ". Value: " << ss.str());
00175 } catch(const boost::bad_lexical_cast &) {
00176 ROS_WARN_STREAM("Wrong address: " << config_.Reg_addr << " " << it);
00177 }
00178 config_.Read_reg = false;
00179 }
00180 if(config_.Write_reg) {
00181
00182 it = strtoul(config_.Reg_addr.c_str(), NULL, 0);
00183 usValue = strtoul(config_.Reg_val.c_str(), NULL, 0);
00184
00185 status = BTAwriteRegister(handle_, it, &usValue, 0);
00186 if (status != BTA_StatusOk) {
00187 ROS_WARN_STREAM("Could not write reg: " <<
00188 config_.Reg_addr <<
00189 " with val: " <<
00190 config_.Reg_val <<
00191 ". Status: " << status);
00192 }
00193 ROS_INFO_STREAM("Written register: " << config_.Reg_addr << ". Value: " << config_.Reg_val);
00194 BTAgetIntegrationTime(handle_, &usValue);
00195 config_.Integration_Time = usValue;
00196 BTAsetFrameRate(handle_, fr);
00197 config_.Frame_rate = fr;
00198 config_.Write_reg = false;
00199
00200 }
00201
00202 }
00203
00204 size_t BtaRos::getDataSize(BTA_DataFormat dataFormat) {
00205 switch (dataFormat) {
00206 case BTA_DataFormatUInt16:
00207 return sizeof(uint16_t);
00208 break;
00209 case BTA_DataFormatSInt16:
00210 return sizeof(int16_t);
00211 break;
00212 case BTA_DataFormatFloat32:
00213 return sizeof(float);
00214 break;
00215 }
00216 }
00217
00218 std::string BtaRos::getDataType(BTA_DataFormat dataFormat) {
00219 switch (dataFormat) {
00220 case BTA_DataFormatUInt16:
00221 return sensor_msgs::image_encodings::TYPE_16UC1;
00222 break;
00223 case BTA_DataFormatSInt16:
00224 return sensor_msgs::image_encodings::TYPE_16SC1;
00225 break;
00226 case BTA_DataFormatFloat32:
00227 return sensor_msgs::image_encodings::TYPE_32FC1;
00228 break;
00229 }
00230 }
00231
00232 float BtaRos::getUnit2Meters(BTA_Unit unit) {
00233
00234 switch (unit) {
00235 case BTA_UnitCentimeter:
00236 return 1/1000.;
00237 break;
00238 case BTA_UnitMillimeter:
00239 return 1/100.;
00240 break;
00241 default:
00242 return 1.0;
00243 break;
00244 }
00245 }
00246
00247 void BtaRos::publishData()
00248 {
00249 if (
00250 (pub_amp_.getNumSubscribers() == 0) &&
00251 (pub_dis_.getNumSubscribers() == 0) &&
00252 (pub_xyz_.getNumSubscribers() == 0)
00253 ) return;
00254
00255 BTA_Status status;
00256
00257 BTA_Frame *frame;
00258 status = BTAgetFrame(handle_, &frame, 3000);
00259 if (status != BTA_StatusOk) {
00260 return;
00261 }
00262
00263 ROS_DEBUG(" frameArrived FrameCounter %d", frame->frameCounter);
00264
00265 BTA_DataFormat dataFormat;
00266 BTA_Unit unit;
00267 uint16_t xRes, yRes;
00268 sensor_msgs::CameraInfoPtr ci_tof(new sensor_msgs::CameraInfo(cim_tof_.getCameraInfo()));
00269 ci_tof->header.frame_id = nodeName_+"/tof_camera";
00270
00271
00272 void *distances;
00273 status = BTAgetDistances(frame, &distances, &dataFormat, &unit, &xRes, &yRes);
00274 if (status == BTA_StatusOk) {
00275 sensor_msgs::ImagePtr dis (new sensor_msgs::Image);
00276 dis->header.seq = frame->frameCounter;
00277 dis->header.stamp.sec = frame->timeStamp;
00278 dis->height = yRes;
00279 dis->width = xRes;
00280 dis->encoding = getDataType(dataFormat);
00281 dis->step = xRes*getDataSize(dataFormat);
00282 dis->data.resize(xRes*yRes*getDataSize(dataFormat));
00283 memcpy ( &dis->data[0], distances, xRes*yRes*getDataSize(dataFormat) );
00284
00285 dis->header.frame_id = "distances";
00286 pub_dis_.publish(dis,ci_tof);
00287 }
00288
00289 bool ampOk = false;
00290 void *amplitudes;
00291 BTA_DataFormat amDataFormat;
00292 status = BTAgetAmplitudes(frame, &litudes,
00293 &amDataFormat, &unit, &xRes, &yRes);
00294 if (status == BTA_StatusOk) {
00295 sensor_msgs::ImagePtr amp (new sensor_msgs::Image);
00296 amp->header.seq = frame->frameCounter;
00297 amp->header.stamp.sec = frame->timeStamp;
00298 amp->height = yRes;
00299 amp->width = xRes;
00300 amp->encoding = getDataType(amDataFormat);
00301 amp->step = xRes*getDataSize(amDataFormat);
00302 amp->data.resize(xRes*yRes*getDataSize(amDataFormat));
00303 memcpy ( &->data[0], amplitudes, xRes*yRes*getDataSize(amDataFormat) );
00304
00305 amp->header.frame_id = "amplitudes";
00306 pub_amp_.publish(amp,ci_tof);
00307 ampOk = true;
00308 }
00309
00310 void *xCoordinates, *yCoordinates, *zCoordinates;
00311 status = BTAgetXYZcoordinates(frame, &xCoordinates, &yCoordinates, &zCoordinates, &dataFormat, &unit, &xRes, &yRes);
00312 if (status == BTA_StatusOk) {
00313 if (_xyz->width != xRes || _xyz->height != yRes || _xyz->fields.size() != 4) {
00314 _xyz->width = xRes;
00315 _xyz->height = yRes;
00316 sensor_msgs::PointCloud2Modifier modifier(*_xyz);
00317 modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
00318 "y", 1, sensor_msgs::PointField::FLOAT32,
00319 "z", 1, sensor_msgs::PointField::FLOAT32,
00320 "intensity", 1, sensor_msgs::PointField::UINT16);
00321 modifier.resize(_xyz->height * _xyz->width);
00322 _xyz->header.frame_id = "cloud";
00323 _xyz->is_dense = true;
00324 }
00325
00326
00327
00328
00329
00330
00331
00332 float conv = getUnit2Meters(unit);
00333 sensor_msgs::PointCloud2Iterator<float> _x(*_xyz, "x");
00334 sensor_msgs::PointCloud2Iterator<float> _y(*_xyz, "y");
00335 sensor_msgs::PointCloud2Iterator<float> _z(*_xyz, "z");
00336 sensor_msgs::PointCloud2Iterator<unsigned short> _i(*_xyz, "intensity");
00337 if (dataFormat == BTA_DataFormatSInt16) {
00338
00339
00340
00341
00342 for (size_t i = 0; i < yRes*xRes; i++, ++_x, ++_y, ++_z, ++_i) {
00343 *_x = static_cast<short *>(xCoordinates)[i]*conv;
00344 *_y = static_cast<short *>(yCoordinates)[i]*conv;
00345 *_z = static_cast<short *>(zCoordinates)[i]*conv;
00346 if (ampOk) {
00347 if(amDataFormat == BTA_DataFormatUInt16) {
00348 *_i = static_cast<unsigned short *>(amplitudes)[i];
00349 } else if(amDataFormat == BTA_DataFormatFloat32) {
00350 *_i = static_cast<float *>(amplitudes)[i];
00351 }
00352 } else
00353 *_i = 255;
00354 }
00355 } else if (dataFormat == BTA_DataFormatFloat32) {
00356 for (size_t i = 0; i < yRes*xRes; i++, ++_x, ++_y, ++_z, ++_i) {
00357 *_x = static_cast<float *>(xCoordinates)[i]*conv;
00358 *_y = static_cast<float *>(yCoordinates)[i]*conv;
00359 *_z = static_cast<float *>(zCoordinates)[i]*conv;
00360 if (ampOk) {
00361 if(amDataFormat == BTA_DataFormatUInt16) {
00362 *_i = static_cast<unsigned short *>(amplitudes)[i];
00363 } else if(amDataFormat == BTA_DataFormatFloat32) {
00364 *_i = static_cast<float *>(amplitudes)[i];
00365 }
00366 } else
00367 *_i = 255;
00368 }
00369 } else {
00370 ROS_WARN_STREAM("Unhandled BTA_DataFormat: " << dataFormat);
00371 return;
00372 }
00373
00374
00375 _xyz->header.seq = frame->frameCounter;
00376 _xyz->header.stamp.sec = frame->timeStamp;
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410 pub_xyz_.publish(_xyz);
00411 }
00412
00413 BTAfreeFrame(&frame);
00414 }
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434 void BtaRos::parseConfig() {
00435 int iusValue;
00436 if(nh_private_.getParam(nodeName_+"/udpDataIpAddrLen",iusValue)) {
00437 config_.udpDataIpAddrLen = (uint8_t)iusValue;
00438 ROS_DEBUG_STREAM("config_.udpDataIpAddrLen: " << (int)config_.udpDataIpAddrLen);
00439 }
00440 if (nh_private_.hasParam(nodeName_+"/udpDataIpAddr")) {
00441 ROS_DEBUG_STREAM("config_.udpDataIpAddr:");
00442 for (int i = 1; i <= config_.udpDataIpAddrLen; i++) {
00443 std::ostringstream to_string;
00444 to_string << "";
00445 to_string << nodeName_ << "/udpDataIpAddr/n" << i;
00446 nh_private_.getParam(to_string.str(),iusValue);
00447 udpDataIpAddr_[i-1] = (uint8_t)iusValue;
00448 ROS_DEBUG_STREAM((int)udpDataIpAddr_[i-1] << ",");
00449 }
00450 config_.udpDataIpAddr = udpDataIpAddr_;
00451 }
00452 if(nh_private_.getParam(nodeName_+"/udpDataPort",iusValue)) {
00453 config_.udpDataPort = (uint16_t)iusValue;
00454 ROS_DEBUG_STREAM("config_.udpDataPort: " << config_.udpDataPort);
00455 }
00456
00457 if(nh_private_.getParam(nodeName_+"/udpControlOutIpAddrLen",iusValue)) {
00458 config_.udpControlOutIpAddrLen = (uint8_t)iusValue;
00459 ROS_DEBUG_STREAM("config_.udpControlOutIpAddrLen: " << (int)config_.udpControlOutIpAddrLen);
00460 }
00461 if (nh_private_.hasParam(nodeName_+"/udpControlOutIpAddr")) {
00462 ROS_DEBUG_STREAM("config_.udpControlOutIpAddr:");
00463 for (int i = 1; i <= config_.udpControlOutIpAddrLen; i++) {
00464 std::ostringstream to_string;
00465 to_string << "";
00466 to_string << nodeName_ << "/udpControlOutIpAddr/n" << i;
00467 nh_private_.getParam(to_string.str(),iusValue);
00468 udpControlOutIpAddr_[i-1] = (uint8_t)iusValue;
00469 ROS_DEBUG_STREAM((int)udpControlOutIpAddr_[i-1] << ",");
00470 }
00471 config_.udpControlOutIpAddr = udpControlOutIpAddr_;
00472 }
00473 if(nh_private_.getParam(nodeName_+"/udpControlOutPort",iusValue)) {
00474 config_.udpControlOutPort = (uint16_t)iusValue;
00475 ROS_DEBUG_STREAM("config_.udpControlOutPort: " << (int)config_.udpControlOutPort);
00476 }
00477
00478 if(nh_private_.getParam(nodeName_+"/udpControlInIpAddrLen",iusValue)) {
00479 config_.udpControlInIpAddrLen = (uint8_t)iusValue;
00480 ROS_DEBUG_STREAM("config_.udpControlInIpAddrLen: " << (int)config_.udpControlInIpAddrLen);
00481 }
00482 if (nh_private_.hasParam(nodeName_+"/udpControlInIpAddr")) {
00483
00484 ROS_DEBUG_STREAM("config_.udpControlInIpAddr:");
00485 for (int i = 1; i <= config_.udpControlInIpAddrLen; i++) {
00486 std::ostringstream to_string;
00487 to_string << "";
00488 to_string << nodeName_ << "/udpControlInIpAddr/n" << i;
00489 nh_private_.getParam(to_string.str(),iusValue);
00490 udpControlInIpAddr_[i-1] = (uint8_t)iusValue;
00491 ROS_DEBUG_STREAM((int)udpControlInIpAddr_[i-1] << ",");
00492 }
00493 config_.udpControlInIpAddr = udpControlInIpAddr_;
00494 }
00495 if(nh_private_.getParam(nodeName_+"/udpControlInPort",iusValue)) {
00496 config_.udpControlInPort = (uint16_t)iusValue;
00497 ROS_DEBUG_STREAM("config_.udpControlInPort: " << (int)config_.udpControlInPort);
00498 }
00499
00500 if(nh_private_.getParam(nodeName_+"/tcpDeviceIpAddrLen",iusValue)) {
00501 config_.tcpDeviceIpAddrLen = (uint8_t)iusValue;
00502 ROS_DEBUG_STREAM("config_.tcpDeviceIpAddrLen: " << (int)config_.tcpDeviceIpAddrLen);
00503 }
00504 if (nh_private_.hasParam(nodeName_+"/tcpDeviceIpAddr")) {
00505 ROS_DEBUG_STREAM("TCP address:");
00506 for (int i = 1; i <= config_.tcpDeviceIpAddrLen; i++) {
00507 std::ostringstream to_string;
00508 to_string << "";
00509 to_string << nodeName_ << "/tcpDeviceIpAddr/n" << i;
00510 nh_private_.getParam(to_string.str(),iusValue);
00511 tcpDeviceIpAddr_[i-1] = (uint8_t)iusValue;
00512 ROS_DEBUG_STREAM((int)tcpDeviceIpAddr_[i-1] << ",");
00513 }
00514 config_.tcpDeviceIpAddr = tcpDeviceIpAddr_;
00515 }
00516 if(nh_private_.getParam(nodeName_+"/tcpControlPort",iusValue)) {
00517 config_.tcpControlPort = (uint16_t)iusValue;
00518 ROS_DEBUG_STREAM("config_.tcpControlPort: " << config_.tcpControlPort);
00519 }
00520
00521 if (nh_private_.getParam(nodeName_+"/tcpDataPort",iusValue))
00522 config_.tcpDataPort = (uint16_t)iusValue;
00523
00524 nh_private_.getParam(nodeName_+"/uartPortName",uartPortName_);
00525 config_.uartPortName = (uint8_t *)uartPortName_.c_str();
00526
00527 if(nh_private_.getParam(nodeName_+"/uartBaudRate",iusValue))
00528 config_.uartBaudRate = (uint32_t)iusValue;
00529 if(nh_private_.getParam(nodeName_+"/uartDataBits",iusValue))
00530 config_.uartDataBits = (uint8_t)iusValue;
00531 if(nh_private_.getParam(nodeName_+"/uartStopBits",iusValue))
00532 config_.uartStopBits = (uint8_t)iusValue;
00533 if(nh_private_.getParam(nodeName_+"/uartParity",iusValue))
00534 config_.uartParity = (uint8_t)iusValue;
00535 if(nh_private_.getParam(nodeName_+"/uartTransmitterAddress",iusValue))
00536 config_.uartTransmitterAddress = (uint8_t)iusValue;
00537 if(nh_private_.getParam(nodeName_+"/uartReceiverAddress",iusValue))
00538 config_.uartReceiverAddress = (uint8_t)iusValue;
00539 if(nh_private_.getParam(nodeName_+"/serialNumber",iusValue))
00540 config_.serialNumber = (uint32_t)iusValue;
00541
00542 nh_private_.getParam(nodeName_+"/calibFileName",calibFileName_);
00543 config_.calibFileName = (uint8_t *)calibFileName_.c_str();
00544
00545 int32_t frameMode;
00546 if (nh_private_.getParam(nodeName_+"/frameMode",frameMode))
00547 config_.frameMode = (BTA_FrameMode)frameMode;
00548
00549 if (nh_private_.getParam(nodeName_+"/verbosity",iusValue))
00550 config_.verbosity = (uint8_t)iusValue;
00551
00552 if(nh_private_.getParam(nodeName_+"/frameQueueLength",iusValue))
00553 config_.frameQueueLength = (uint16_t)iusValue;
00554
00555 int32_t frameQueueMode;
00556 if (nh_private_.getParam(nodeName_+"/frameQueueMode",frameQueueMode))
00557 config_.frameQueueMode = (BTA_QueueMode)frameQueueMode;
00558
00559 #if !defined(BTA_ETH) || !defined(BTA_P100)
00560 int32_t deviceType;
00561 if(nh_private_.getParam(nodeName_+"/deviceType",deviceType))
00562 config_.deviceType = (BTA_DeviceType)deviceType;
00563 #endif
00564
00565
00566 config_.infoEvent = &infoEventCb;
00567 }
00568
00569
00570
00571 int BtaRos::connectCamera() {
00572 BTA_Status status;
00573 BTA_DeviceInfo *deviceInfo;
00574
00575
00576
00577 status = (BTA_Status)-1;
00578 for (int i=0; i<10; i++) {
00579 ROS_INFO_STREAM("Connecting... try " << i+1);
00580 status = BTAopen(&config_, &handle_);
00581 if (status != BTA_StatusOk) {
00582 if (!nh_private_.ok() && ros::isShuttingDown())
00583 return -1;
00584 ROS_WARN_STREAM("Could not connect to the camera. status: " << status);
00585 continue;
00586 }
00587 break;
00588 }
00589 if (!BTAisConnected(handle_)) {
00590 ROS_WARN_STREAM("Could not connect to the camera.");
00591 return -1;
00592 }
00593
00594 ROS_INFO_STREAM("Camera connected sucessfully. status: " << status);
00595 status = BTAgetDeviceInfo(handle_, &deviceInfo);
00596 if (status != BTA_StatusOk) {
00597 ROS_WARN_STREAM("Could not get device info. status: " << status);
00598 return status;
00599 }
00600
00601 ROS_INFO_STREAM("Retrieved device info: \n"
00602 << "deviceType: " << deviceInfo->deviceType << "\n"
00603 << "serialNumber: " << deviceInfo->serialNumber << "\n"
00604 << "firmwareVersionMajor: " << deviceInfo->firmwareVersionMajor << "\n"
00605 << "firmwareVersionMinor: " << deviceInfo->firmwareVersionMinor << "\n"
00606 << "firmwareVersionNonFunc: " << deviceInfo->firmwareVersionNonFunc
00607 << "\n");
00608
00609 ROS_INFO_STREAM("Service running: " << (int)BTAisRunning(handle_));
00610 ROS_INFO_STREAM("Connection up: " << (int)BTAisConnected(handle_));
00611
00612 BTAfreeDeviceInfo(deviceInfo);
00613 return 1;
00614 }
00615
00616 int BtaRos::initialize()
00617 {
00618
00619
00620
00621
00622
00623 BTAinitConfig(&config_);
00624
00625 parseConfig();
00626
00627
00628
00629
00630 ROS_DEBUG_STREAM("Config Readed sucessfully");
00631
00632 BTA_Status status;
00633 if (connectCamera() < 0)
00634 return -1;
00635
00636 reconfigure_server_.reset(new ReconfigureServer(nh_private_));
00637 reconfigure_server_->setCallback(boost::bind(&BtaRos::callback, this, _1, _2));
00638
00639 while (!config_init_)
00640 {
00641 ROS_DEBUG("Waiting for dynamic reconfigure configuration.");
00642 boost::this_thread::sleep(boost::posix_time::seconds(0.1));
00643 }
00644 ROS_DEBUG("Dynamic reconfigure configuration received.");
00645
00646
00647
00648
00649 {
00650
00651 cim_tof_.setCameraName(nodeName_);
00652 if(cim_tof_.validateURL(
00653 "package://bta_ros/calib.yml")) {
00654 cim_tof_.loadCameraInfo("package://bta_ros/calib.yml");
00655 ROS_INFO_STREAM("Loaded camera calibration from " <<
00656 "package://bta_ros/calib.yml" );
00657 } else {
00658 ROS_WARN_STREAM("Camera info at: " <<
00659 "package://bta_ros/calib.yml" <<
00660 " not found. Using an uncalibrated config_.");
00661 }
00662
00663 pub_amp_ = it_.advertiseCamera(nodeName_ + "/tof_camera/image_raw", 1);
00664 pub_dis_ = it_.advertiseCamera(nodeName_ + "/tof_camera/compressedDepth", 1);
00665 pub_xyz_ = nh_private_.advertise<sensor_msgs::PointCloud2> (nodeName_ + "/tof_camera/point_cloud_xyz", 1);
00666
00667
00668
00669 }
00670
00671 while (nh_private_.ok() && !ros::isShuttingDown()) {
00672 if (!BTAisConnected(handle_)) {
00673 ROS_WARN_STREAM("The camera got disconnected." << BTAisConnected(handle_));
00674 if (connectCamera() < 0)
00675 break;
00676 }
00677
00678 publishData();
00679 ros::spinOnce ();
00680 }
00681 return 0;
00682 }
00683 }
00684