bta_ros.hpp
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 
00042 #ifndef _BTA_ROS_HPP_
00043 #define _BTA_ROS_HPP_
00044 
00045 #include <bta.h>
00046 
00047 // ROS communication
00048 #include <ros/ros.h>
00049 #include <image_transport/image_transport.h>
00050 #include <camera_info_manager/camera_info_manager.h>
00051 #include <sensor_msgs/Image.h>
00052 #include <sensor_msgs/CameraInfo.h>
00053 #include <sensor_msgs/SetCameraInfo.h>
00054 #include <sensor_msgs/image_encodings.h>
00055 #include <sensor_msgs/PointCloud2.h>
00056 #include <sensor_msgs/point_cloud2_iterator.h>
00057 #include <tf2_ros/static_transform_broadcaster.h>
00058 #include <geometry_msgs/TransformStamped.h>
00059 
00060 #include <ros/console.h>
00061 #include <tf/transform_listener.h>
00062 
00063 // PCL 
00064 #include <pcl_ros/point_cloud.h>
00065 #include <pcl/point_types.h>
00066 #include <pcl/conversions.h>
00067 
00068 // Standard libs
00069 #include <stdio.h>
00070 #include <time.h>
00071 #include <sstream>
00072 #include <string>
00073 
00074 // Dynamic reconfigure
00075 #include <bta_ros/bta_rosConfig.h>
00076 #include <dynamic_reconfigure/server.h>
00077 
00078 //static ros::Publisher int_amp,int_dis,int_rgb;
00079 
00080 namespace bta_ros {
00081 
00082 class BtaRos
00083 {
00084 
00085     typedef bta_ros::bta_rosConfig Config;
00086     typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00087 
00088 public:
00089 
00099     BtaRos(ros::NodeHandle nh_camera, ros::NodeHandle nh_private, std::string nodeName);
00100 
00106     virtual ~BtaRos();
00107 
00113     int initialize();
00114 
00120     int connectCamera();
00121 
00127     void close();
00128 
00129 
00135     void publishData();
00136 
00137     //void ampCb(const sensor_msgs::ImagePtr& amp);
00138 
00139     //void disCb(const sensor_msgs::ImagePtr& dis);
00140 
00141     static void BTA_CALLCONV infoEventCb(BTA_EventId eventId, int8_t *msg) {
00142         ROS_DEBUG("   Callback: infoEvent (%d) %s\n", eventId, msg);
00143     }
00144 
00145 private:
00146     ros::NodeHandle nh_, nh_private_;
00147     std::string nodeName_;
00148     camera_info_manager::CameraInfoManager cim_tof_/*, *cim_rgb*/;
00149     image_transport::ImageTransport it_;
00150     image_transport::CameraPublisher pub_amp_, pub_dis_/*, pub_rgb*/;
00151     tf2_ros::StaticTransformBroadcaster pub_tf;
00152     geometry_msgs::TransformStamped transformStamped;
00153     ros::Publisher pub_xyz_;
00154     //ros::Subscriber sub_amp_, sub_dis_;
00155     boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00156     bool config_init_;
00157 
00158     boost::mutex connect_mutex_;
00159 
00160     // Variables needed for config
00161     uint8_t udpDataIpAddr_[6], udpControlOutIpAddr_[6],
00162     udpControlInIpAddr_[6], tcpDeviceIpAddr_[6];
00163     std::string uartPortName_, calibFileName_;
00164 
00165     sensor_msgs::PointCloud2Ptr _xyz;
00166 
00167     BTA_Handle handle_;
00168     BTA_Config config_;
00169 
00179     void callback(bta_ros::bta_rosConfig &config, uint32_t level);
00180 
00186     void parseConfig();
00187 
00193     size_t getDataSize(BTA_DataFormat dataFormat);
00194 
00200     std::string getDataType(BTA_DataFormat dataFormat);
00201 
00207     float getUnit2Meters(BTA_Unit unit);
00208 
00209     //Test of using camera callback
00210     /*static void BTA_CALLCONV frameArrived(BTA_Frame *frame) {
00211 
00212         ROS_DEBUG("   Callback: frameArrived FrameCounter %d\n", frame->frameCounter);
00213 
00214         BTA_Status status;
00215         uint16_t *distances;
00216         BTA_DataFormat dataFormat;
00217         BTA_Unit unit;
00218         uint16_t xRes, yRes;
00219 
00220         sensor_msgs::ImagePtr amp (new sensor_msgs::Image);
00221         sensor_msgs::ImagePtr dis (new sensor_msgs::Image);
00222 
00223         status = BTAgetDistances(frame, (void **)&distances, &dataFormat, &unit, &xRes, &yRes);
00224         if (status == BTA_StatusOk) {
00225                 if (dataFormat == BTA_DataFormatUInt16) {
00226                     if (unit == BTA_UnitMillimeter) {
00227                                 dis->header.seq = frame->frameCounter;
00228                                 dis->header.stamp.sec = frame->timeStamp;
00229                                 dis->height = yRes;
00230                                 dis->width = xRes;
00231                                 dis->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00232                                 dis->step = yRes*sizeof(uint16_t);
00233                                 dis->data.resize(xRes*yRes*sizeof(uint16_t));
00234                                 memcpy ( &dis->data[0], distances, xRes*yRes*sizeof(uint16_t) );
00235                     }
00236                 }
00237         }
00238 
00239         uint16_t *amplitudes;
00240         status = BTAgetAmplitudes(frame, (void **)&amplitudes, &dataFormat, &unit, &xRes, &yRes);
00241         if (status == BTA_StatusOk) {
00242                 if (dataFormat == BTA_DataFormatUInt16) {
00243                     if (unit == BTA_UnitUnitLess) {
00244                         dis->header.seq = frame->frameCounter;
00245                                 amp->header.stamp.sec = frame->timeStamp;
00246                                 amp->height = yRes;
00247                                 amp->width = xRes;
00248                                 amp->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00249                                 amp->step = yRes*sizeof(uint16_t);
00250                                 amp->data.resize(xRes*yRes*sizeof(uint16_t));
00251                                 memcpy ( &amp->data[0], amplitudes, xRes*yRes*sizeof(uint16_t) );
00252                     }
00253                 }
00254         }
00255 
00256         / *
00257          * Publishing the messages
00258          * /
00259         //int_amp.publish(amp);
00260         //int_dis.publish(dis);
00261 
00262     };*/
00263 
00264 };
00265 }
00266 
00267 #endif
00268 


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