bta_ros.cpp
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     _xyz (new sensor_msgs::PointCloud2)
00058 {
00059     //Set log to debug to test capturing. Remove if not needed.
00060     /*
00061     if( ros::console::set_logger_level(
00062                 ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) )
00063     {
00064         ros::console::notifyLoggerLevelsChanged();
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     // Check the configuretion parameters with those given in the initialization
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         //std::stringstream ss;
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     //ROS_INFO_STREAM("BTA_Unit: " << unit);
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, &amplitudes,
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 ( &amp->data[0], amplitudes, xRes*yRes*getDataSize(amDataFormat) );
00304 
00305         amp->header.frame_id = "amplitudes";//nodeName_+"/tof_camera";
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         //if (_cloud.size() != yRes*xRes) {
00326         //    _cloud.resize(yRes*xRes);
00327         //}
00328         /* else {
00329             pub_xyz_.publish(_xyz);
00330             return;
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             /*short *xC, *yC, *zC;
00339             xC = (short *)xCoordinates;
00340             yC = (short *)yCoordinates;
00341             zC = (short *)zCoordinates;*/
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         //pcl::toROSMsg(_cloud, *_xyz);
00374 
00375         _xyz->header.seq = frame->frameCounter;
00376         _xyz->header.stamp.sec = frame->timeStamp;
00377 
00378         //Keeping until resolving problem with rviz
00379         /*
00380             xyz->width = xRes;
00381             xyz->height = yRes;
00382             sensor_msgs::PointCloud2Modifier modifier(*xyz);
00383             ROS_DEBUG_STREAM("MAL? " << unit);
00384 
00385             modifier.setPointCloud2Fields(3, "x", 1, sensor_msgs::PointField::INT16,
00386                                               "y", 1, sensor_msgs::PointField::INT16,
00387                                               "z", 1, sensor_msgs::PointField::INT16/*,
00388                                               "intensity", 1, sensor_msgs::PointField::UINT16* /);
00389             ROS_DEBUG_STREAM("MAL? " << modifier.size());
00390             short *xC, *yC, *zC;
00391             xC = (short *)xCoordinates;
00392             yC = (short *)yCoordinates;
00393             zC = (short *)zCoordinates;
00394             std::vector<short> test;
00395             for (size_t i = 0; i < yRes*xRes; i++) {
00396               test.push_back(xC[i]);
00397               test.push_back(yC[i]);
00398               test.push_back(zC[i]);
00399               //memcpy ( &xyz->data[i*xyz->point_step], &xCoordinates[i*getDataSize(dataFormat)], getDataSize(dataFormat) );
00400               //memcpy ( &xyz->data[i*xyz->point_step+getDataSize(dataFormat) ], &yCoordinates[i*getDataSize(dataFormat)], getDataSize(dataFormat) );
00401               //memcpy ( &xyz->data[i*xyz->point_step+2*getDataSize(dataFormat) ], &zCoordinates[i*getDataSize(dataFormat)], getDataSize(dataFormat) );
00402               //memcpy ( &xyz->data[i*xyz->point_step+3*getDataSize(dataFormat)], &amplitudes[i*getDataSize(amDataFormat)], getDataSize(amDataFormat) );
00403               //ROS_DEBUG_STREAM("ox: " << (short)xCoordinates[i*getDataSize(dataFormat)] << " y: " << (short)yCoordinates[i*getDataSize(dataFormat)] << " z: " << (short)zCoordinates[i*getDataSize(dataFormat)]);
00404               //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) ]);
00405            }
00406            memcpy ( &xyz->data[0], &test[0], test.size());
00407            ROS_DEBUG_STREAM("MAL? " << modifier.size());
00408                 */
00409 
00410         pub_xyz_.publish(_xyz);
00411     }
00412 
00413     BTAfreeFrame(&frame);
00414 }
00415 
00416 /*void BtaRos::ampCb(const sensor_msgs::ImagePtr& amp)
00417           {
00418                 sensor_msgs::CameraInfoPtr ci_tof(new sensor_msgs::CameraInfo(cim_tof_.getCameraInfo()));
00419                         ci_tof->header.frame_id = nodeName_+"/tof_camera";
00420                         amp->header.frame_id = nodeName_+"/tof_camera";
00421 
00422                         pub_amp_.publish(amp,ci_tof);
00423           }
00424 
00425           void BtaRos::disCb(const sensor_msgs::ImagePtr& dis)
00426           {
00427                 sensor_msgs::CameraInfoPtr ci_tof(new sensor_msgs::CameraInfo(cim_tof_.getCameraInfo()));
00428                         ci_tof->header.frame_id = nodeName_+"/tof_camera";
00429                         dis->header.frame_id = nodeName_+"/tof_camera";
00430 
00431                         pub_dis_.publish(dis,ci_tof);
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     //config_.frameArrived = &frameArrived;
00566     config_.infoEvent = &infoEventCb;
00567 }
00568 
00569 
00570 
00571 int BtaRos::connectCamera() {
00572     BTA_Status status;
00573     BTA_DeviceInfo *deviceInfo;
00574 
00575     // Init camera connection
00576     //ros::Duration().sleep();
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                          * Camera config
00621                          */
00622 
00623     BTAinitConfig(&config_);
00624 
00625     parseConfig();
00626 
00627     /*
00628                          * Camera Initialization
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                          * ROS Node Initialization
00648                          */
00649     {
00650         // Advertise all published topics
00651         cim_tof_.setCameraName(nodeName_);
00652         if(cim_tof_.validateURL(
00653                     /*camera_info_url_*/"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         //sub_amp_ = nh_private_.subscribe("bta_node_amp", 1, &BtaRos::ampCb, this);
00668         //sub_dis_ = nh_private_.subscribe("bta_node_dis", 1, &BtaRos::disCb, this);
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 


bta_ros
Author(s): Angel Merino , Simon Vogl
autogenerated on Thu Jun 2 2016 04:07:04