bta_ros: bta_ros.cpp Source File

Go to the documentation of this file.
00001 /******************************************************************************
00002  * Copyright (c) 2014
00003  * VoXel Interaction Design GmbH
00004  *
00005  * @author Angel Merino Sastre
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy
00008  * of this software and associated documentation files (the "Software"), to deal
00009  * in the Software without restriction, including without limitation the rights
00010  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00011  * copies of the Software, and to permit persons to whom the Software is
00012  * furnished to do so, subject to the following conditions:
00013  *
00014  * The above copyright notice and this permission notice shall be included in
00015  * all copies or substantial portions of the Software.
00016  *
00017  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00020  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00022  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00023  * THE SOFTWARE.
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         {
00058                 //Set log to debug to test capturing. Remove if not needed.
00059                 if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 
00060                     ros::console::levels::Debug) ) {
00061                 ros::console::notifyLoggerLevelsChanged();
00062                 }
00063         }
00064 
00065         BtaRos::~BtaRos() 
00066         {
00067                 printf("clossing!!!!!");
00068                 close();
00069         }
00070 
00071         void BtaRos::close() 
00072         {
00073                 ROS_DEBUG("Close called");
00074                 if (BTAisConnected(handle_)) {
00075                         ROS_DEBUG("Clossing..");
00076                         BTA_Status status;
00077                         status = BTAclose(&handle_);
00078                         printf("done: %d \n", status);
00079                 }
00080                 
00081                 return;
00082         }
00083 
00084         
00085         void BtaRos::callback(bta_ros::bta_rosConfig &config_, uint32_t level)
00086         {
00087                 BTA_Status status;
00088                 // Check the configuretion parameters with those given in the initialization
00089                 int it;
00090                 double fr;
00091                 uint32_t usValue;
00092 
00093                 if (!config_init_) {
00094                         if (nh_private_.getParam(nodeName_+"/integrationTime",it)) {
00095                                 usValue = (uint32_t)it;
00096                                 status = BTAsetIntegrationTime(handle_, usValue);
00097                                 if (status != BTA_StatusOk)
00098                                         ROS_WARN_STREAM("Error setting IntegrationTime:: " << status << "---------------");     
00099                         } else {
00100                 status = BTAgetIntegrationTime(handle_, &usValue);
00101                 if (status != BTA_StatusOk)
00102                                         ROS_WARN_STREAM("Error reading IntegrationTime: " << status << "---------------");
00103                                 else
00104                                         nh_private_.setParam(nodeName_+"/integrationTime", (int)usValue);
00105                         }
00106                         nh_private_.getParam(nodeName_+"/integrationTime",config_.Integration_Time);
00107                         
00108                         if (nh_private_.getParam(nodeName_+"/frameRate",fr)) {
00109                                 status = BTAsetFrameRate(handle_, fr);
00110                                 if (status != BTA_StatusOk)
00111                                         ROS_WARN_STREAM("Error setting FrameRate: " << status << "---------------");    
00112                         } else {
00113                                 float fr_f;
00114                                 status = BTAgetFrameRate(handle_, &fr_f);
00115                                 fr = fr_f;
00116                                 if (status != BTA_StatusOk)
00117                                         ROS_WARN_STREAM("Error reading FrameRate: " << status << "---------------");
00118                                 else
00119                                         nh_private_.setParam(nodeName_+"/frameRate", fr);
00120                         }
00121                         nh_private_.getParam(nodeName_+"/frameRate",config_.Frame_rate);
00122             config_init_ = true;
00123             return;
00124                 }
00125                 
00126                 
00127                 nh_private_.getParam(nodeName_+"/integrationTime",it);
00128                 if(it != config_.Integration_Time) {
00129                         usValue = (uint32_t)config_.Integration_Time;
00130                         status = BTAsetIntegrationTime(handle_, usValue);
00131                                 if (status != BTA_StatusOk)
00132                                         ROS_WARN_STREAM("Error setting IntegrationTime: " << status << "---------------");      
00133                                 else
00134                                         nh_private_.setParam(nodeName_+"/integrationTime", config_.Integration_Time);
00135                 }
00136                 
00137                 nh_private_.getParam(nodeName_+"/frameRate",fr);
00138                 if(fr != config_.Frame_rate) {
00139                         usValue = (uint32_t)config_.Frame_rate;
00140                         status = BTAsetFrameRate(handle_, usValue);
00141                                 if (status != BTA_StatusOk)
00142                                         ROS_WARN_STREAM("Error setting FrameRate: " << status << "---------------");    
00143                                 else
00144                                         nh_private_.setParam(nodeName_+"/frameRate", config_.Frame_rate);
00145                 }
00146 
00147     if(config_.Read_reg) {
00148         try {
00149         std::stringstream ss;
00150         it = strtoul(config_.Reg_addr.c_str(), NULL, 0);
00151         status = BTAreadRegister(handle_, it, &usValue, 0);
00152         if (status != BTA_StatusOk) {
00153             ROS_WARN_STREAM("Could not read reg: " << config_.Reg_addr << ". Status: " << status);
00154         }
00155         ss<<"";
00156         ss  << "0x"<< std::hex << usValue;
00157         ss >> config_.Reg_val;
00158         ROS_INFO_STREAM("Read register: " << config_.Reg_addr << ". Value: " << ss.str());
00159         } catch(const boost::bad_lexical_cast &) {
00160         ROS_WARN_STREAM("Wrong address: " << config_.Reg_addr << " " << it);
00161         }
00162         config_.Read_reg = false;
00163     }
00164     if(config_.Write_reg) {
00165       //std::stringstream ss;
00166       it = strtoul(config_.Reg_addr.c_str(), NULL, 0);
00167       usValue = strtoul(config_.Reg_val.c_str(), NULL, 0);
00168 
00169       status = BTAwriteRegister(handle_, it, &usValue, 0);
00170       if (status != BTA_StatusOk) {
00171         ROS_WARN_STREAM("Could not write reg: " <<
00172           config_.Reg_addr <<
00173           " with val: " <<
00174           config_.Reg_val <<
00175           ". Status: " << status);
00176       }
00177       ROS_INFO_STREAM("Written register: " << config_.Reg_addr << ". Value: " << config_.Reg_val);
00178       BTAgetIntegrationTime(handle_, &usValue);
00179       config_.Integration_Time = usValue;
00180       BTAsetFrameRate(handle_, fr);
00181       config_.Frame_rate = fr;
00182       config_.Write_reg = false;
00183 
00184     }
00185 
00186         }
00187 
00188   size_t BtaRos::getDataSize(BTA_DataFormat dataFormat) {
00189         switch (dataFormat) {
00190       case BTA_DataFormatUInt16:
00191         return sizeof(uint16_t);
00192         break;
00193       case BTA_DataFormatSInt16:
00194         return sizeof(int16_t);
00195         break;
00196       case BTA_DataFormatFloat32:
00197         return sizeof(float);
00198         break;
00199     }
00200   }
00201   
00202   std::string BtaRos::getDataType(BTA_DataFormat dataFormat) {
00203     switch (dataFormat) {
00204       case BTA_DataFormatUInt16:
00205         return sensor_msgs::image_encodings::TYPE_16UC1;
00206         break;
00207       case BTA_DataFormatSInt16:
00208         return sensor_msgs::image_encodings::TYPE_16SC1;
00209         break;
00210       case BTA_DataFormatFloat32:
00211         return sensor_msgs::image_encodings::TYPE_32FC1;
00212         break;
00213     }
00214   }
00215 
00216         void BtaRos::publishData() 
00217         {
00218 
00219                 BTA_Status status;
00220 
00221                 BTA_Frame *frame;
00222                 status = BTAgetFrame(handle_, &frame, 3000);
00223                 if (status != BTA_StatusOk) {
00224                         return;
00225                 }
00226 
00227                 ROS_DEBUG("             frameArrived FrameCounter %d", frame->frameCounter);
00228 
00229                 BTA_DataFormat dataFormat;
00230                 BTA_Unit unit;
00231                 uint16_t xRes, yRes;
00232                 sensor_msgs::CameraInfoPtr ci_tof(new sensor_msgs::CameraInfo(cim_tof_.getCameraInfo()));
00233                 ci_tof->header.frame_id = nodeName_+"/tof_camera";
00234                 
00235                 
00236           void *distances;
00237           status = BTAgetDistances(frame, &distances, &dataFormat, &unit, &xRes, &yRes);
00238           if (status == BTA_StatusOk) {
00239             sensor_msgs::ImagePtr dis (new sensor_msgs::Image);
00240             dis->header.seq = frame->frameCounter;
00241       dis->header.stamp.sec = frame->timeStamp;
00242       dis->height = yRes;
00243       dis->width = xRes;
00244       dis->encoding = getDataType(dataFormat);
00245       dis->step = xRes*getDataSize(dataFormat);
00246       dis->data.resize(xRes*yRes*getDataSize(dataFormat));
00247       memcpy ( &dis->data[0], distances, xRes*yRes*getDataSize(dataFormat) );
00248             
00249             dis->header.frame_id = nodeName_+"/tof_camera";
00250             pub_dis_.publish(dis,ci_tof);
00251           }
00252         
00253           bool ampOk = false;
00254           void *amplitudes;
00255           BTA_DataFormat amDataFormat;
00256           status = BTAgetAmplitudes(frame, &amplitudes, 
00257                                                                 &amDataFormat, &unit, &xRes, &yRes);
00258           if (status == BTA_StatusOk) {
00259             sensor_msgs::ImagePtr amp (new sensor_msgs::Image);
00260       amp->header.seq = frame->frameCounter;
00261       amp->header.stamp.sec = frame->timeStamp;
00262       amp->height = yRes;
00263       amp->width = xRes;
00264       amp->encoding = getDataType(amDataFormat);
00265       amp->step = xRes*getDataSize(amDataFormat);
00266       amp->data.resize(xRes*yRes*getDataSize(amDataFormat));
00267       memcpy ( &amp->data[0], amplitudes, xRes*yRes*getDataSize(amDataFormat) );
00268 
00269             amp->header.frame_id = nodeName_+"/tof_camera";
00270             pub_amp_.publish(amp,ci_tof);
00271             ampOk = true;
00272           }
00273 
00274         void *xCoordinates, *yCoordinates, *zCoordinates;
00275   status = BTAgetXYZcoordinates(frame, &xCoordinates, &yCoordinates, &zCoordinates, &dataFormat, &unit, &xRes, &yRes);
00276   if (status == BTA_StatusOk) {
00277     sensor_msgs::PointCloud2Ptr xyz (new sensor_msgs::PointCloud2);
00278     pcl::PointCloud<pcl::PointXYZI> cloud;
00279     if (dataFormat == BTA_DataFormatSInt16) {
00280       short *xC, *yC, *zC;
00281       xC = (short *)xCoordinates; 
00282       yC = (short *)yCoordinates;
00283       zC = (short *)zCoordinates;
00284       
00285       for (size_t i = 0; i < yRes*xRes; i++) {
00286         pcl::PointXYZI temp_point;
00287         temp_point.x = xC[i];
00288         temp_point.y = yC[i];
00289         temp_point.z = zC[i];
00290         if (ampOk) {
00291                 if(amDataFormat == BTA_DataFormatUInt16) {
00292                         unsigned short *A;
00293                         A = (unsigned short *)amplitudes;
00294                         temp_point.intensity = A[i];
00295                 }
00296                 if(amDataFormat == BTA_DataFormatFloat32) {
00297                         temp_point.intensity = ((float *)amplitudes)[i];
00298                 }
00299         } else
00300                 temp_point.intensity = 255;
00301         
00302         cloud.push_back(temp_point);
00303         }
00304                 }
00305     if (dataFormat == BTA_DataFormatFloat32) {
00306         float *xC, *yC, *zC;
00307       xC = (float *)xCoordinates;
00308       yC = (float *)yCoordinates;
00309       zC = (float *)zCoordinates;
00310 
00311       for (size_t i = 0; i < yRes*xRes; i++) {
00312         pcl::PointXYZI temp_point;
00313                                 temp_point.x = xC[i];
00314                                 temp_point.y = yC[i];
00315                                 temp_point.z = zC[i];
00316                         if (ampOk) {
00317               if(amDataFormat == BTA_DataFormatUInt16) {
00318                                         unsigned short *A;
00319                                         A = (unsigned short *)amplitudes; 
00320                                         temp_point.intensity = A[i];
00321                                 }
00322         if(amDataFormat == BTA_DataFormatFloat32) {
00323                 temp_point.intensity = ((float *)amplitudes)[i];
00324         }
00325      }
00326       cloud.push_back(temp_point);
00327              
00328         }
00329                 }
00330     pcl::toROSMsg(cloud, *xyz);
00331     
00332     xyz->header.seq = frame->frameCounter;
00333     xyz->header.stamp.sec = frame->timeStamp;
00334     xyz->header.frame_id = nodeName_+"/tof_camera";
00335     xyz->is_dense = true;
00336     
00337     //Keeping until resolving problem with rviz
00338                 /*
00339     xyz->width = xRes;
00340     xyz->height = yRes;
00341     sensor_msgs::PointCloud2Modifier modifier(*xyz);
00342     ROS_DEBUG_STREAM("MAL? " << unit);
00343      
00344     modifier.setPointCloud2Fields(3, "x", 1, sensor_msgs::PointField::INT16,
00345                                       "y", 1, sensor_msgs::PointField::INT16,
00346                                       "z", 1, sensor_msgs::PointField::INT16/*,
00347                                       "intensity", 1, sensor_msgs::PointField::UINT16* /);
00348     ROS_DEBUG_STREAM("MAL? " << modifier.size());
00349     short *xC, *yC, *zC;
00350     xC = (short *)xCoordinates;
00351     yC = (short *)yCoordinates;
00352     zC = (short *)zCoordinates;
00353     std::vector<short> test;
00354     for (size_t i = 0; i < yRes*xRes; i++) {
00355       test.push_back(xC[i]);
00356       test.push_back(yC[i]);
00357       test.push_back(zC[i]);
00358       //memcpy ( &xyz->data[i*xyz->point_step], &xCoordinates[i*getDataSize(dataFormat)], getDataSize(dataFormat) );
00359       //memcpy ( &xyz->data[i*xyz->point_step+getDataSize(dataFormat) ], &yCoordinates[i*getDataSize(dataFormat)], getDataSize(dataFormat) );
00360       //memcpy ( &xyz->data[i*xyz->point_step+2*getDataSize(dataFormat) ], &zCoordinates[i*getDataSize(dataFormat)], getDataSize(dataFormat) );
00361       //memcpy ( &xyz->data[i*xyz->point_step+3*getDataSize(dataFormat)], &amplitudes[i*getDataSize(amDataFormat)], getDataSize(amDataFormat) );
00362       //ROS_DEBUG_STREAM("ox: " << (short)xCoordinates[i*getDataSize(dataFormat)] << " y: " << (short)yCoordinates[i*getDataSize(dataFormat)] << " z: " << (short)zCoordinates[i*getDataSize(dataFormat)]);
00363       //ROS_DEBUG_STREAM("x: " << (short)xyz->data[i*xyz->point_step] << " y: " << (short)xyz->data[i*xyz->point_step+getDataSize(dataFormat) ] << " z: " << (short)xyz->data[i*xyz->point_step+2*getDataSize(dataFormat) ]);
00364    }
00365    memcpy ( &xyz->data[0], &test[0], test.size());
00366    ROS_DEBUG_STREAM("MAL? " << modifier.size());
00367         */
00368 
00369     pub_xyz_.publish(xyz);
00370     } 
00371         
00372                 BTAfreeFrame(&frame);
00373 
00374         }
00375         
00376   /*void BtaRos::ampCb(const sensor_msgs::ImagePtr& amp)
00377   {
00378         sensor_msgs::CameraInfoPtr ci_tof(new sensor_msgs::CameraInfo(cim_tof_.getCameraInfo()));
00379                 ci_tof->header.frame_id = nodeName_+"/tof_camera";
00380                 amp->header.frame_id = nodeName_+"/tof_camera";
00381         
00382                 pub_amp_.publish(amp,ci_tof);
00383   }
00384     
00385   void BtaRos::disCb(const sensor_msgs::ImagePtr& dis)
00386   {
00387         sensor_msgs::CameraInfoPtr ci_tof(new sensor_msgs::CameraInfo(cim_tof_.getCameraInfo()));
00388                 ci_tof->header.frame_id = nodeName_+"/tof_camera";
00389                 dis->header.frame_id = nodeName_+"/tof_camera";
00390         
00391                 pub_dis_.publish(dis,ci_tof);
00392   }*/
00393 
00394         void BtaRos::parseConfig() {
00395                 int iusValue;
00396                 if(nh_private_.getParam(nodeName_+"/udpDataIpAddrLen",iusValue)) {
00397                   config_.udpDataIpAddrLen = (uint8_t)iusValue;
00398                         ROS_DEBUG_STREAM("config_.udpDataIpAddrLen: " << (int)config_.udpDataIpAddrLen);
00399                 }
00400                 if (nh_private_.hasParam(nodeName_+"/udpDataIpAddr")) {
00401                         ROS_DEBUG_STREAM("config_.udpDataIpAddr:");
00402                         for (int i = 1; i <= config_.udpDataIpAddrLen; i++) {
00403                                 std::ostringstream to_string;
00404                                 to_string << "";
00405                                 to_string << nodeName_ << "/udpDataIpAddr/n" << i;
00406                                 nh_private_.getParam(to_string.str(),iusValue);
00407                                 udpDataIpAddr_[i-1] = (uint8_t)iusValue;
00408                                 ROS_DEBUG_STREAM((int)udpDataIpAddr_[i-1] << ","); 
00409                         }
00410                         config_.udpDataIpAddr = udpDataIpAddr_;
00411                 }
00412                 if(nh_private_.getParam(nodeName_+"/udpDataPort",iusValue)) {
00413                   config_.udpDataPort = (uint16_t)iusValue;
00414                   ROS_DEBUG_STREAM("config_.udpDataPort: " << config_.udpDataPort);  
00415                 }
00416                 
00417                 if(nh_private_.getParam(nodeName_+"/udpControlOutIpAddrLen",iusValue)) {
00418                         config_.udpControlOutIpAddrLen = (uint8_t)iusValue;
00419                   ROS_DEBUG_STREAM("config_.udpControlOutIpAddrLen: " << (int)config_.udpControlOutIpAddrLen);
00420                 }
00421                 if (nh_private_.hasParam(nodeName_+"/udpControlOutIpAddr")) {
00422                         ROS_DEBUG_STREAM("config_.udpControlOutIpAddr:");
00423                         for (int i = 1; i <= config_.udpControlOutIpAddrLen; i++) {
00424                                 std::ostringstream to_string;
00425                                 to_string << "";
00426                                 to_string << nodeName_ << "/udpControlOutIpAddr/n" << i;
00427                                 nh_private_.getParam(to_string.str(),iusValue);
00428                                 udpControlOutIpAddr_[i-1] = (uint8_t)iusValue;
00429                                 ROS_DEBUG_STREAM((int)udpControlOutIpAddr_[i-1] << ","); 
00430                         }
00431                         config_.udpControlOutIpAddr = udpControlOutIpAddr_;
00432                 }
00433                 if(nh_private_.getParam(nodeName_+"/udpControlOutPort",iusValue)) {
00434                         config_.udpControlOutPort = (uint16_t)iusValue; 
00435                   ROS_DEBUG_STREAM("config_.udpControlOutPort: " << (int)config_.udpControlOutPort);
00436                 }
00437                 
00438                 if(nh_private_.getParam(nodeName_+"/udpControlInIpAddrLen",iusValue)) {
00439                         config_.udpControlInIpAddrLen = (uint8_t)iusValue;
00440                   ROS_DEBUG_STREAM("config_.udpControlInIpAddrLen: " << (int)config_.udpControlInIpAddrLen);
00441                 }
00442                 if (nh_private_.hasParam(nodeName_+"/udpControlInIpAddr")) {
00443                         
00444                         ROS_DEBUG_STREAM("config_.udpControlInIpAddr:");
00445                         for (int i = 1; i <= config_.udpControlInIpAddrLen; i++) {
00446                                 std::ostringstream to_string;
00447                                 to_string << "";
00448                                 to_string << nodeName_ << "/udpControlInIpAddr/n" << i;
00449                                 nh_private_.getParam(to_string.str(),iusValue);
00450                                 udpControlInIpAddr_[i-1] = (uint8_t)iusValue; 
00451                                 ROS_DEBUG_STREAM((int)udpControlInIpAddr_[i-1] << ","); 
00452                         }
00453                         config_.udpControlInIpAddr = udpControlInIpAddr_;
00454                 }
00455                 if(nh_private_.getParam(nodeName_+"/udpControlInPort",iusValue)) {
00456                         config_.udpControlInPort = (uint16_t)iusValue; 
00457                   ROS_DEBUG_STREAM("config_.udpControlInPort: " << (int)config_.udpControlInPort);
00458                 }
00459           
00460                 if(nh_private_.getParam(nodeName_+"/tcpDeviceIpAddrLen",iusValue)) {
00461                   config_.tcpDeviceIpAddrLen = (uint8_t)iusValue;
00462                   ROS_DEBUG_STREAM("config_.tcpDeviceIpAddrLen: " << (int)config_.tcpDeviceIpAddrLen);
00463                 }
00464                 if (nh_private_.hasParam(nodeName_+"/tcpDeviceIpAddr")) {
00465                         ROS_DEBUG_STREAM("TCP address:");
00466                         for (int i = 1; i <= config_.tcpDeviceIpAddrLen; i++) {
00467                                 std::ostringstream to_string;
00468                                 to_string << "";
00469                                 to_string << nodeName_ << "/tcpDeviceIpAddr/n" << i;
00470                                 nh_private_.getParam(to_string.str(),iusValue);
00471                                 tcpDeviceIpAddr_[i-1] = (uint8_t)iusValue;
00472                                 ROS_DEBUG_STREAM((int)tcpDeviceIpAddr_[i-1] << ",");  
00473                         }
00474                         config_.tcpDeviceIpAddr = tcpDeviceIpAddr_;
00475                 }
00476                 if(nh_private_.getParam(nodeName_+"/tcpControlPort",iusValue)) {
00477                   config_.tcpControlPort = (uint16_t)iusValue;
00478                   ROS_DEBUG_STREAM("config_.tcpControlPort: " << config_.tcpControlPort);
00479                 }
00480                 
00481                 if (nh_private_.getParam(nodeName_+"/tcpDataPort",iusValue))  
00482                         config_.tcpDataPort = (uint16_t)iusValue;
00483         
00484                 nh_private_.getParam(nodeName_+"/uartPortName",uartPortName_);
00485                 config_.uartPortName = (int8_t *)uartPortName_.c_str();
00486         
00487                 if(nh_private_.getParam(nodeName_+"/uartBaudRate",iusValue))
00488                         config_.uartBaudRate = (uint32_t)iusValue;
00489                 if(nh_private_.getParam(nodeName_+"/uartDataBits",iusValue))
00490                         config_.uartDataBits = (uint8_t)iusValue;
00491                 if(nh_private_.getParam(nodeName_+"/uartStopBits",iusValue))
00492                         config_.uartStopBits = (uint8_t)iusValue;
00493                 if(nh_private_.getParam(nodeName_+"/uartParity",iusValue))
00494                         config_.uartParity = (uint8_t)iusValue;
00495                 if(nh_private_.getParam(nodeName_+"/uartTransmitterAddress",iusValue))
00496                         config_.uartTransmitterAddress = (uint8_t)iusValue;
00497                 if(nh_private_.getParam(nodeName_+"/uartReceiverAddress",iusValue))
00498                         config_.uartReceiverAddress = (uint8_t)iusValue;
00499                 if(nh_private_.getParam(nodeName_+"/serialNumber",iusValue))
00500                         config_.serialNumber = (uint32_t)iusValue;
00501 
00502                 nh_private_.getParam(nodeName_+"/calibFileName",calibFileName_);
00503                 config_.calibFileName = (uint8_t *)calibFileName_.c_str();
00504         
00505                 int32_t frameMode;
00506                 if (nh_private_.getParam(nodeName_+"/frameMode",frameMode))
00507                   config_.frameMode = (BTA_FrameMode)frameMode;
00508         
00509                 if (nh_private_.getParam(nodeName_+"/verbosity",iusValue))
00510                   config_.verbosity = (uint8_t)iusValue;
00511         
00512                 if(nh_private_.getParam(nodeName_+"/frameQueueLength",iusValue))
00513                   config_.frameQueueLength = (uint16_t)iusValue;
00514         
00515                 int32_t frameQueueMode;
00516                 if (nh_private_.getParam(nodeName_+"/frameQueueMode",frameQueueMode))
00517                 config_.frameQueueMode = (BTA_QueueMode)frameQueueMode;
00518         
00519                 //config_.frameArrived = &frameArrived;
00520                 config_.infoEvent = &infoEventCb;
00521         }
00522 
00523 
00524 
00525         int BtaRos::connectCamera() {
00526                 BTA_Status status;
00527                 BTA_DeviceInfo *deviceInfo;
00528                 
00529                 // Init camera connection
00530                 //ros::Duration().sleep();
00531                 status = (BTA_Status)-1;
00532                 for (int i=0; i<10; i++) {
00533                         ROS_DEBUG_STREAM("Connecting... try " << i+1);
00534                         status = BTAopen(&config_, &handle_);
00535                         if (status != BTA_StatusOk) {
00536                                 if (!nh_private_.ok() && ros::isShuttingDown())
00537                                         return -1;
00538                                 ROS_WARN_STREAM("Could not connect to the camera. status: " << status);
00539                                 continue;
00540                         }
00541                         break;
00542                 }
00543                 if (!BTAisConnected(handle_)) {
00544                                 ROS_WARN_STREAM("Could not connect to the camera.");
00545                                 return -1;
00546                 }
00547                 
00548                 ROS_INFO_STREAM("Camera connected sucessfully. status: " << status);
00549                 status = BTAgetDeviceInfo(handle_, &deviceInfo);
00550                 if (status != BTA_StatusOk) {
00551                         ROS_WARN_STREAM("Could not get device info. status: " << status);
00552                     return status;
00553                 }
00554                 
00555                 ROS_DEBUG_STREAM("Retrieved device info: \n" 
00556                         << "deviceType: " << deviceInfo->deviceType << "\n"
00557                         << "serialNumber: " << deviceInfo->serialNumber << "\n"
00558                         << "firmwareVersionMajor: " << deviceInfo->firmwareVersionMajor << "\n"
00559                         << "firmwareVersionMinor: " << deviceInfo->firmwareVersionMinor << "\n"
00560                         << "firmwareVersionNonFunc: " << deviceInfo->firmwareVersionNonFunc 
00561                         << "\n");
00562 
00563                 ROS_INFO_STREAM("Service running: " << (int)BTAisRunning(handle_));
00564                 ROS_INFO_STREAM("Connection up: " << (int)BTAisConnected(handle_));
00565                 
00566                 BTAfreeDeviceInfo(deviceInfo);
00567                 return 1;
00568         }
00569         
00570         int BtaRos::initialize() 
00571         {
00572 
00573                 /*
00574                  * Camera config
00575                  */
00576                 
00577                 BTAinitConfig(&config_);  
00578         
00579                 parseConfig();
00580                 
00581                 /*
00582                  * Camera Initialization
00583                  */
00584                 ROS_DEBUG_STREAM("Config Readed sucessfully");
00585                 
00586                 BTA_Status status;
00587                 if (connectCamera() < 0)
00588                         return -1;
00589                 
00590                 reconfigure_server_.reset(new ReconfigureServer(nh_private_));
00591                 reconfigure_server_->setCallback(boost::bind(&BtaRos::callback, this, _1, _2));
00592                 
00593                 while (!config_init_)
00594                 {
00595                         ROS_DEBUG("Waiting for dynamic reconfigure configuration.");
00596                         boost::this_thread::sleep(boost::posix_time::seconds(0.1));
00597                 }
00598                 ROS_DEBUG("Dynamic reconfigure configuration received.");
00599                 
00600                 /*
00601                  * ROS Node Initialization
00602                  */
00603                  {
00604                 // Advertise all published topics
00605                         cim_tof_.setCameraName(nodeName_);
00606                         if(cim_tof_.validateURL(
00607                                         /*camera_info_url_*/"package://bta_ros/calib.yml")) {
00608                                 cim_tof_.loadCameraInfo("package://bta_ros/calib.yml");
00609                                 ROS_INFO_STREAM("Loaded camera calibration from " << 
00610                                         "package://bta_ros/calib.yml"   );
00611                         } else {
00612                                 ROS_WARN_STREAM("Camera info at: " <<
00613                                         "package://bta_ros/calib.yml" <<
00614                                         " not found. Using an uncalibrated config_.");
00615                         }       
00616                 
00617                         pub_amp_ = it_.advertiseCamera(nodeName_ + "/tof_camera/image_raw", 1);
00618                         pub_dis_ = it_.advertiseCamera(nodeName_ + "/tof_camera/compressedDepth", 1);
00619                         pub_xyz_ = nh_private_.advertise<sensor_msgs::PointCloud2> (nodeName_ + "/tof_camera/point_cloud_xyz", 1);
00620         
00621                         //sub_amp_ = nh_private_.subscribe("bta_node_amp", 1, &BtaRos::ampCb, this);
00622                         //sub_dis_ = nh_private_.subscribe("bta_node_dis", 1, &BtaRos::disCb, this);
00623                 }
00624                 
00625                 while (nh_private_.ok() && !ros::isShuttingDown()) {
00626                         if (!BTAisConnected(handle_)) {
00627                                 ROS_WARN_STREAM("The camera got disconnected." << BTAisConnected(handle_));
00628                                 if (connectCamera() < 0)
00629                                         break;
00630                         }
00631                                 
00632                         publishData();
00633                         ros::spinOnce ();       
00634                 }
00635                 return 0;
00636         }
00637 }
00638