imi_Driver.cpp
Go to the documentation of this file.
1 #include "imi_Driver.h"
2 #include "imi_Device.h"
3 #include <unistd.h>
4 #include <stdlib.h>
5 #include <stdio.h>
6 #include <sys/shm.h>
7 #include <time.h>
10 #include "ImiDefines.h"
11 #include "ImiProperties.h"
12 #include <boost/thread/thread.hpp>
13 
14 namespace imi_wrapper
15 {
16 
17  static const int64_t NUM_MILLISEC_PER_SEC = 1000;
18  static const int64_t NUM_MICROSECS_PER_SEC = 1000000;
19  static const int64_t NUM_NANNOSECS_PER_SEC = 1000000000;
20 
21  static const int64_t NUM_MICROSECS_PER_MILLESEC = NUM_MICROSECS_PER_SEC /
23  static const int64_t NUM_NANOSECS_PER_MILLESEC = NUM_NANNOSECS_PER_SEC /
25  static const int64_t NUM_NANOSECS_PER_MICROSEC = NUM_NANNOSECS_PER_SEC /
27 
28  uint64_t imi_timeNanos()
29  {
30  uint64_t ticks = 0;
31  struct timespec ts;
32  clock_gettime (CLOCK_MONOTONIC, &ts);
33  ticks = NUM_NANNOSECS_PER_SEC * static_cast<uint64_t> (ts.tv_sec) +
34  static_cast<uint64_t> (ts.tv_nsec);
35 
36  return ticks;
37  }
38  uint64_t imi_time()
39  {
40  return static_cast<uint64_t> (imi_timeNanos() / NUM_NANOSECS_PER_MILLESEC);
41  }
42 
43  uint64_t depthStartTime = 0;
44  uint64_t depthCount = 0;
45 
46  uint64_t uvcStartTime = 0;
47  uint64_t uvcCount = 0;
48 
49  ImiFrameMode supportFrameMode[] = {{IMI_PIXEL_FORMAT_DEP_16BIT, 640, 480, 16, 30},
50  {IMI_PIXEL_FORMAT_DEP_16BIT, 320, 240, 16, 30}};
51 
52  ImiCameraFrameMode supportCameraFrameMode[] = {{CAMERA_PIXEL_FORMAT_RGB888, 640, 480, 30},
53  {CAMERA_PIXEL_FORMAT_RGB888, 960, 720, 30},
54  {CAMERA_PIXEL_FORMAT_RGB888, 1280, 720, 30},
55  {CAMERA_PIXEL_FORMAT_RGB888, 1920, 1080, 30}};
56 
58  nh_(n),
59  pnh_(pnh),
60  color_subscribers_(false),
61  depth_subscribers_(false),
62  cloud_subscribers_(false),
63  uvc_subscribers_(false),
64  config_init(false),
65  depthMode(0),
66  colorMode(0)
67  {
69  reconfigure_server_->setCallback(boost::bind(&ImiDriver::configCb, this, _1, _2));
70  while (!config_init)
71  {
72  ROS_DEBUG("Waiting for dynamic reconfigure configuration.");
73  boost::this_thread::sleep(boost::posix_time::milliseconds(100));
74  }
75 
76  pImiDevice = new ImiDevice();
77  int ret = initDevice();
78  if (ret == 0)
79  {
81  }
82  }
83 
85  {
86  if (pImiDevice != NULL)
87  {
88  delete pImiDevice;
89  }
90  }
91 
92  void ImiDriver::configCb(Config &config, uint32_t level)
93  {
94  depthMode = config.depth_mode - 1;
95  colorMode = config.color_mode - 1;
96  printf("************ %d %d\n", config.depth_mode, config.color_mode);
97  config_init = true;
98  }
99 
101  {
102  int ret = pImiDevice->openDevice();
103  if (ret != 0)
104  {
106  }
107 
108  return ret;
109  }
110 
112  {
113  ImiFrameMode frameMode;
114  frameMode.pixelFormat = supportFrameMode[depthMode].pixelFormat;
115  frameMode.resolutionX = supportFrameMode[depthMode].resolutionX;
116  frameMode.resolutionY = supportFrameMode[depthMode].resolutionY;
117  frameMode.bitsPerPixel = supportFrameMode[depthMode].bitsPerPixel;
118  frameMode.framerate = supportFrameMode[depthMode].framerate;
119  pImiDevice->startDepthStream(frameMode);
120 
121  }
122 
124  {
125  ImiFrameMode frameMode;
126  frameMode.pixelFormat = IMI_PIXEL_FORMAT_IMAGE_RGB24;
127  frameMode.resolutionX = 640;
128  frameMode.resolutionY = 480;
129  frameMode.bitsPerPixel = 16;
130  frameMode.framerate = 30;
131  pImiDevice->startColorStream(frameMode);
132  }
133 
135  {
136  ImiCameraFrameMode frameMode;
137  frameMode.pixelFormat = supportCameraFrameMode[colorMode].pixelFormat;
138  frameMode.resolutionX = supportCameraFrameMode[colorMode].resolutionX;
139  frameMode.resolutionY = supportCameraFrameMode[colorMode].resolutionY;
140  frameMode.fps = supportCameraFrameMode[colorMode].fps;
141  pImiDevice->startUVCStream(frameMode);
142  }
143 
144 
145  void ImiDriver::newColorFrameCallback(sensor_msgs::ImagePtr image)
146  {
147  if (color_subscribers_)
148  {
149  sensor_msgs::CameraInfoPtr info = pImiDevice->getDefaultCameraInfo(640, 480);
150 
151  timespec timenow;
152  clock_gettime(CLOCK_REALTIME, &timenow);
153 
154  //info->header.stamp.sec = timenow.tv_sec;
155  //info->header.stamp.nsec = timenow.tv_nsec;
156  //image->header.stamp.sec = info->header.stamp.sec;
157  //image->header.stamp.nsec = info->header.stamp.nsec;
158 
159  image->header.stamp = ros::Time::now();
160  info->header.stamp = image->header.stamp;
161  //printf("******header.stamp.sec=%lld header.stamp.nsec=%lld\n", image->header.stamp.sec, image->header.stamp.nsec);
162 
163  info->header.frame_id = image->header.frame_id;
164  pub_color_.publish(image, info);
165  }
166  }
167 
168  void ImiDriver::newUVCFrameCallback(sensor_msgs::ImagePtr image)
169  {
170  if (uvc_subscribers_)
171  {
172  uvcCount++;
173 
174  if(uvcStartTime == 0)
175  {
176  uvcStartTime = imi_time();
177  }
178 
179  sensor_msgs::CameraInfoPtr info = pImiDevice->getDefaultCameraInfo(image->width, image->height);
180 
181  timespec timenow;
182  clock_gettime(CLOCK_REALTIME, &timenow);
183 
184  //info->header.stamp.sec = timenow.tv_sec;
185  //info->header.stamp.nsec = timenow.tv_nsec;
186  //image->header.stamp.sec = info->header.stamp.sec;
187  //image->header.stamp.nsec = info->header.stamp.nsec;
188  image->header.stamp = ros::Time::now();
189  info->header.stamp = image->header.stamp;
190  //printf("******header.stamp.sec=%lld header.stamp.nsec=%lld\n", image->header.stamp.sec, image->header.stamp.nsec);
191 
192  info->header.frame_id = image->header.frame_id;
193  pub_uvc_.publish(image, info);
194 
195  if (imi_time()-uvcStartTime >= 10000)
196  {
197  //printf("****** UVC FPS = %f\n", uvcCount/10.0f);
198  uvcStartTime = imi_time();
199  uvcCount = 0;
200  }
201  }
202  }
203 
204  void ImiDriver::newDepthFrameCallback(sensor_msgs::ImagePtr image)
205  {
206 
207  if (depth_subscribers_)
208  {
209  depthCount++;
210 
211  if(depthStartTime == 0)
212  {
213  depthStartTime = imi_time();
214  }
215  sensor_msgs::CameraInfoPtr info = pImiDevice->getDefaultCameraInfo(image->width, image->height);
216 
217  timespec timenow;
218  clock_gettime(CLOCK_REALTIME, &timenow);
219 
220  //info->header.stamp.sec = timenow.tv_sec;
221  //info->header.stamp.nsec = timenow.tv_nsec;
222  //image->header.stamp.sec = info->header.stamp.sec;
223  //image->header.stamp.nsec = info->header.stamp.nsec;
224  image->header.stamp = ros::Time::now();
225  info->header.stamp = image->header.stamp;
226  //printf("******header.stamp.sec=%lld header.stamp.nsec=%lld\n", image->header.stamp.sec, image->header.stamp.nsec);
227  info->header.frame_id = image->header.frame_id;
228 
229  pub_depth_.publish(image, info);
230 
231  if (imi_time()-depthStartTime >= 10000)
232  {
233  //printf("****** Depth FPS = %f\n", depthCount/10.0f);
234  depthStartTime = imi_time();
235  depthCount = 0;
236  }
237  }
238  }
239 
241  {
242  if (cloud_subscribers_)
243  {
244  pub_cloud_.publish(cloudPtr);
245  }
246  }
247 
248 
250  {
252 
253  if (color_subscribers_)
254  {
257  printf("****** color_subscribers_ in \n");
258  }
259  else
260  {
263  printf("****** color_subscribers_ out \n");
264  }
265  }
266 
268  {
270 
271  if (uvc_subscribers_)
272  {
274  startUVCStream();
275  printf("****** uvc_subscribers_ in \n");
276  uvcStartTime = 0;
277  uvcCount = 0;
278  }
279  else
280  {
283  printf("****** uvc_subscribers_ out \n");
284  }
285  }
286 
288  {
290 
291  if (depth_subscribers_)
292  {
295  printf("****** depth_subscribers_ in \n");
296  depthStartTime = 0;
297  depthCount = 0;
298  }
299  else
300  {
303  printf("****** depth_subscribers_ out \n");
304  }
305  }
306 
308  {
310 
311  if (cloud_subscribers_)
312  {
315  printf("****** cloud_subscribers_ in \n");
316  }
317  else
318  {
321  printf("****** cloud_subscribers_ out \n");
322  }
323  }
324 
325 
327  {
328  ros::NodeHandle color_nh(nh_, "rgb");
329  image_transport::ImageTransport color_it(color_nh);
330 
331  ros::NodeHandle uvc_nh(nh_, "uvc");
332  image_transport::ImageTransport uvc_it(uvc_nh);
333 
334  ros::NodeHandle depth_nh(nh_, "depth");
335  image_transport::ImageTransport depth_it(depth_nh);
336 
337  ros::NodeHandle cloud_it(nh_, "cloud");
338 
339  if (pImiDevice->hasColorSensor())
340  {
342  ros::SubscriberStatusCallback rssc = boost::bind(&ImiDriver::colorConnectCb, this);
343  pub_color_ = color_it.advertiseCamera("image", 1, itssc, itssc);
344  }
345 
346  if (pImiDevice->hasUVCSensor())
347  {
349  ros::SubscriberStatusCallback rssc = boost::bind(&ImiDriver::uvcConnectCb, this);
350  pub_uvc_ = uvc_it.advertiseCamera("image", 1, itssc, itssc);
351  }
352 
353  if (pImiDevice->hasDepthSensor())
354  {
356  ros::SubscriberStatusCallback rssc = boost::bind(&ImiDriver::depthConnectCb, this);
357  pub_depth_ = depth_it.advertiseCamera("image", 1, itssc, itssc);
358 
359  ros::SubscriberStatusCallback itssc_cloud = boost::bind(&ImiDriver::couldCb, this);
360  pub_cloud_ = cloud_it.advertise<sensor_msgs::PointCloud2>("image", 1, itssc_cloud, itssc_cloud);
361  }
362  }
363 
364 }
image_transport::CameraPublisher pub_color_
Definition: imi_Driver.h:49
static const int64_t NUM_NANOSECS_PER_MICROSEC
Definition: imi_Driver.cpp:25
uint64_t uvcStartTime
Definition: imi_Driver.cpp:46
void setUVCFrameCallback(FrameCallbackFunction callback)
Definition: imi_Device.cpp:436
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: imi_Driver.h:21
uint32_t getNumSubscribers() const
ros::NodeHandle & nh_
Definition: imi_Driver.h:41
bool hasDepthSensor() const
Definition: imi_Device.cpp:425
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void newUVCFrameCallback(sensor_msgs::ImagePtr image)
Definition: imi_Driver.cpp:168
int startDepthStream(ImiFrameMode depthFrameMode)
Definition: imi_Device.cpp:500
int startColorStream(ImiFrameMode colorFrameMode)
Definition: imi_Device.cpp:454
void setCloudPointCallback(CloudCallbackFunction callback)
Definition: imi_Device.cpp:448
ImiFrameMode supportFrameMode[]
Definition: imi_Driver.cpp:49
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
uint64_t depthStartTime
Definition: imi_Driver.cpp:43
ImiDevice * pImiDevice
Definition: imi_Driver.h:43
static const int64_t NUM_MILLISEC_PER_SEC
Definition: imi_Driver.cpp:17
int startUVCStream(ImiCameraFrameMode uvcframeMode)
Definition: imi_Device.cpp:478
boost::shared_ptr< ReconfigureServer > reconfigure_server_
Definition: imi_Driver.h:53
ImiCameraFrameMode supportCameraFrameMode[]
Definition: imi_Driver.cpp:52
static const int64_t NUM_MICROSECS_PER_SEC
Definition: imi_Driver.cpp:18
ros::NodeHandle & pnh_
Definition: imi_Driver.h:42
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool hasColorSensor() const
Definition: imi_Device.cpp:415
image_transport::CameraPublisher pub_uvc_
Definition: imi_Driver.h:52
ImiDriver(ros::NodeHandle &n, ros::NodeHandle &pnh)
Definition: imi_Driver.cpp:57
static const int64_t NUM_MICROSECS_PER_MILLESEC
Definition: imi_Driver.cpp:21
uint64_t depthCount
Definition: imi_Driver.cpp:44
void setColorFrameCallback(FrameCallbackFunction callback)
Definition: imi_Device.cpp:430
sensor_msgs::CameraInfoPtr getDefaultCameraInfo(int width, int height)
Definition: imi_Device.cpp:346
void setDepthFrameCallback(FrameCallbackFunction callback)
Definition: imi_Device.cpp:442
uint32_t getNumSubscribers() const
uint64_t imi_time()
Definition: imi_Driver.cpp:38
uint64_t imi_timeNanos()
Definition: imi_Driver.cpp:28
static Time now()
ros::Publisher pub_cloud_
Definition: imi_Driver.h:51
uint64_t uvcCount
Definition: imi_Driver.cpp:47
image_transport::CameraPublisher pub_depth_
Definition: imi_Driver.h:50
void newColorFrameCallback(sensor_msgs::ImagePtr image)
Definition: imi_Driver.cpp:145
bool hasUVCSensor() const
Definition: imi_Device.cpp:420
void newCloudPointCallback(boost::shared_ptr< sensor_msgs::PointCloud2 > cloudPtr)
Definition: imi_Driver.cpp:240
imi_ros_cfg::ImiConfig Config
Definition: imi_Driver.h:20
void newDepthFrameCallback(sensor_msgs::ImagePtr image)
Definition: imi_Driver.cpp:204
static const int64_t NUM_NANNOSECS_PER_SEC
Definition: imi_Driver.cpp:19
void configCb(Config &config, uint32_t level)
Definition: imi_Driver.cpp:92
static const int64_t NUM_NANOSECS_PER_MILLESEC
Definition: imi_Driver.cpp:23
#define ROS_DEBUG(...)


imi_camera
Author(s): hjimi , HUAJIEIMI
autogenerated on Mon Jun 10 2019 13:32:59