bta_tof_driver.hpp
Go to the documentation of this file.
1 /******************************************************************************
2  * Copyright (c) 2016
3  * VoXel Interaction Design GmbH
4  *
5  * @author Angel Merino Sastre
6  *
7  * Permission is hereby granted, free of charge, to any person obtaining a copy
8  * of this software and associated documentation files (the "Software"), to deal
9  * in the Software without restriction, including without limitation the rights
10  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11  * copies of the Software, and to permit persons to whom the Software is
12  * furnished to do so, subject to the following conditions:
13  *
14  * The above copyright notice and this permission notice shall be included in
15  * all copies or substantial portions of the Software.
16  *
17  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
23  * THE SOFTWARE.
24  *
25  ******************************************************************************/
26 
43 #ifndef _BTA_TOF_DRIVER_HPP_
44 #define _BTA_TOF_DRIVER_HPP_
45 
46 #include <bta.h>
47 
48 // ROS communication
49 #include <ros/ros.h>
52 #include <sensor_msgs/Image.h>
53 #include <sensor_msgs/CameraInfo.h>
54 #include <sensor_msgs/SetCameraInfo.h>
56 #include <sensor_msgs/PointCloud2.h>
59 #include <geometry_msgs/TransformStamped.h>
60 
61 #include <ros/console.h>
62 #include <tf/transform_listener.h>
63 
64 // PCL
65 #include <pcl_ros/point_cloud.h>
66 #include <pcl/point_types.h>
67 #include <pcl/conversions.h>
68 
69 // Standard libs
70 #include <stdio.h>
71 #include <time.h>
72 #include <sstream>
73 #include <string>
74 
75 // Dynamic reconfigure
76 #include <bta_tof_driver/bta_tof_driverConfig.h>
77 #include <dynamic_reconfigure/server.h>
78 
79 //static ros::Publisher int_amp,int_dis,int_rgb;
80 
81 namespace bta_tof_driver {
82 
83 class BtaRos
84 {
85 
86  typedef bta_tof_driver::bta_tof_driverConfig Config;
87  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
88 
89 public:
90 
100  BtaRos(ros::NodeHandle nh_camera, ros::NodeHandle nh_private, std::string nodeName);
101 
107  virtual ~BtaRos();
108 
114  int initialize();
115 
121  int connectCamera();
122 
128  void close();
129 
130 
136  void publishData();
137 
138  //void ampCb(const sensor_msgs::ImagePtr& amp);
139 
140  //void disCb(const sensor_msgs::ImagePtr& dis);
141 
142  static void BTA_CALLCONV infoEventCb(BTA_EventId eventId, int8_t *msg) {
143  ROS_DEBUG(" Callback: infoEvent (%d) %s\n", eventId, msg);
144  }
145 
146 private:
148  std::string nodeName_;
153  geometry_msgs::TransformStamped transformStamped;
155  //ros::Subscriber sub_amp_, sub_dis_;
158 
159  boost::mutex connect_mutex_;
160 
161  // Variables needed for config
165 
166  sensor_msgs::PointCloud2Ptr _xyz;
167 
168  BTA_Handle handle_;
169  BTA_Config config_;
170 
180  void callback(bta_tof_driver::bta_tof_driverConfig &config, uint32_t level);
181 
187  void parseConfig();
188 
194  size_t getDataSize(BTA_DataFormat dataFormat);
195 
201  std::string getDataType(BTA_DataFormat dataFormat);
202 
208  float getUnit2Meters(BTA_Unit unit);
209 
210  //Test of using camera callback
211  /*static void BTA_CALLCONV frameArrived(BTA_Frame *frame) {
212 
213  ROS_DEBUG(" Callback: frameArrived FrameCounter %d\n", frame->frameCounter);
214 
215  BTA_Status status;
216  uint16_t *distances;
217  BTA_DataFormat dataFormat;
218  BTA_Unit unit;
219  uint16_t xRes, yRes;
220 
221  sensor_msgs::ImagePtr amp (new sensor_msgs::Image);
222  sensor_msgs::ImagePtr dis (new sensor_msgs::Image);
223 
224  status = BTAgetDistances(frame, (void **)&distances, &dataFormat, &unit, &xRes, &yRes);
225  if (status == BTA_StatusOk) {
226  if (dataFormat == BTA_DataFormatUInt16) {
227  if (unit == BTA_UnitMillimeter) {
228  dis->header.seq = frame->frameCounter;
229  dis->header.stamp.sec = frame->timeStamp;
230  dis->height = yRes;
231  dis->width = xRes;
232  dis->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
233  dis->step = yRes*sizeof(uint16_t);
234  dis->data.resize(xRes*yRes*sizeof(uint16_t));
235  memcpy ( &dis->data[0], distances, xRes*yRes*sizeof(uint16_t) );
236  }
237  }
238  }
239 
240  uint16_t *amplitudes;
241  status = BTAgetAmplitudes(frame, (void **)&amplitudes, &dataFormat, &unit, &xRes, &yRes);
242  if (status == BTA_StatusOk) {
243  if (dataFormat == BTA_DataFormatUInt16) {
244  if (unit == BTA_UnitUnitLess) {
245  dis->header.seq = frame->frameCounter;
246  amp->header.stamp.sec = frame->timeStamp;
247  amp->height = yRes;
248  amp->width = xRes;
249  amp->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
250  amp->step = yRes*sizeof(uint16_t);
251  amp->data.resize(xRes*yRes*sizeof(uint16_t));
252  memcpy ( &amp->data[0], amplitudes, xRes*yRes*sizeof(uint16_t) );
253  }
254  }
255  }
256 
257  / *
258  * Publishing the messages
259  * /
260  //int_amp.publish(amp);
261  //int_dis.publish(dis);
262 
263  };*/
264 
265 };
266 }
267 
268 #endif //_BTA_TOF_DRIVER_HPP_
269 
image_transport::CameraPublisher pub_amp_
void close()
Closes the connection to the device.
camera_info_manager::CameraInfoManager cim_tof_
static void BTA_CALLCONV infoEventCb(BTA_EventId eventId, int8_t *msg)
ros::NodeHandle nh_private_
geometry_msgs::TransformStamped transformStamped
boost::shared_ptr< ReconfigureServer > reconfigure_server_
image_transport::CameraPublisher pub_dis_
sensor_msgs::PointCloud2Ptr _xyz
virtual ~BtaRos()
Class destructor.
int connectCamera()
Helper for connect to the device.
size_t getDataSize(BTA_DataFormat dataFormat)
Returns the size of the data based in BTA_DataFormat.
void publishData()
Publish the data based on set up parameters.
bta_tof_driver::bta_tof_driverConfig Config
dynamic_reconfigure::Server< Config > ReconfigureServer
void parseConfig()
Reads configuration from the server parameters.
image_transport::ImageTransport it_
tf2_ros::StaticTransformBroadcaster pub_tf
float getUnit2Meters(BTA_Unit unit)
Gives the conversion value to meters from the BTA_Unit.
BtaRos(ros::NodeHandle nh_camera, ros::NodeHandle nh_private, std::string nodeName)
Class constructor.
void callback(bta_tof_driver::bta_tof_driverConfig &config, uint32_t level)
Callback for rqt_reconfigure. It is called any time we change a parameter in the visual interface...
#define ROS_DEBUG(...)
std::string getDataType(BTA_DataFormat dataFormat)
Returns the data encoding flat based in BTA_DataFormat.
int initialize()
Initializes the device and parameters.


bta_tof_driver
Author(s): Angel Merino , Simon Vogl
autogenerated on Sat Nov 23 2019 03:52:25