nm33_node.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <rospack/rospack.h>
4 #include <sensor_msgs/SetCameraInfo.h>
7 
8 #include <opencv/cv.h>
9 #include <cv_bridge/cv_bridge.h>
10 
11 #include <dynamic_reconfigure/server.h>
12 #include <opt_camera/OptNM33CameraConfig.h>
14 
16 {
17 private:
18  // camera
20  std::string serial_id_;
21 
22  // ros
25  int count_;
26 
27  // Publications
32 
33  // Dynamic reconfigure
34  typedef dynamic_reconfigure::Server<opt_camera::OptNM33CameraConfig> ReconfigureServer;
35  ReconfigureServer reconfigure_server_;
36  opt_camera::OptNM33CameraConfig config_;
37  int pan;
38 
39 public:
40  OptCamNode(ros::NodeHandle &node) : node_(node)
41  {
42  // camera device
43  ros::NodeHandle nh("~");
44  int camera_index = 0;
45 
46  nh.getParam("camera_index", camera_index);
47  camera_ = new OptNM3xCamera(camera_index);
48  if ((serial_id_ = camera_->getSerialID()) == "") {
49  nh.getParam("serial_id", serial_id_);
50  }
51  camera_->setSmallHemisphere(1);
52  camera_->setLocationAbsolute(0, 0, 0, 0, 0);
53  camera_->setLocationAbsolute(1, 0, 0, 0, 40);
54  camera_->setLocationAbsolute(2, 0, 0, 0, 120);
55 
56  // image header
57  img_.header.frame_id = std::string("head_camera");
58  img_omni_.header.frame_id = std::string("head_camera");
59  img_wide_.header.frame_id = std::string("head_camera");
60  img_middle_.header.frame_id = std::string("head_camera");
61  img_narrow_.header.frame_id = std::string("head_camera");
62  img_panorama_.header.frame_id = std::string("head_camera");
63  nh.getParam("frame_id", img_.header.frame_id);
64  nh.getParam("omni_frame_id", img_omni_.header.frame_id);
65  nh.getParam("wide_frame_id", img_wide_.header.frame_id);
66  nh.getParam("middle_frame_id", img_middle_.header.frame_id);
67  nh.getParam("narrow_frame_id", img_narrow_.header.frame_id);
68  nh.getParam("panorama_frame_id", img_panorama_.header.frame_id);
69 
70  // info
71  info_.header.frame_id = img_.header.frame_id;
72  info_omni_.header.frame_id = img_omni_.header.frame_id;
73  info_wide_.header.frame_id = img_wide_.header.frame_id;
74  info_middle_.header.frame_id = img_middle_.header.frame_id;
75  info_narrow_.header.frame_id = img_narrow_.header.frame_id;
76  info_panorama_.header.frame_id = img_panorama_.header.frame_id;
77 
78  // advertise
79  img_pub_ = image_transport::ImageTransport(node).advertiseCamera("image_raw" , 1);
80  img_omni_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"omni")).advertiseCamera("image_raw" , 1);
81  img_wide_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"wide")).advertiseCamera("image_raw" , 1);
82  img_middle_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"middle")).advertiseCamera("image_raw" , 1);
83  img_narrow_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"narrow")).advertiseCamera("image_raw" , 1);
84  img_panorama_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"panorama")).advertiseCamera("image_raw" , 1);
85 
86  info_srv_ = node.advertiseService("set_camera_info", &OptCamNode::set_camera_info, this);
87  info_omni_srv_ = ros::NodeHandle(node,"omni").advertiseService("set_camera_info", &OptCamNode::set_camera_info_omni, this);
88  info_wide_srv_ = ros::NodeHandle(node,"wide").advertiseService("set_camera_info", &OptCamNode::set_camera_info_wide, this);
89  info_middle_srv_ = ros::NodeHandle(node,"middle").advertiseService("set_camera_info", &OptCamNode::set_camera_info_middle, this);
90  info_narrow_srv_ = ros::NodeHandle(node,"narrow").advertiseService("set_camera_info", &OptCamNode::set_camera_info_narrow, this);
91  info_panorama_srv_ = ros::NodeHandle(node,"panorama").advertiseService("set_camera_info", &OptCamNode::set_camera_info_panorama, this);
92 
93  // get camera info
94  get_camera_info_method("",info_);
95  get_camera_info_method("-omni",info_omni_);
96  get_camera_info_method("-wide",info_wide_);
97  get_camera_info_method("-middle",info_middle_);
98  get_camera_info_method("-narrow",info_narrow_);
99  get_camera_info_method("-panorama",info_panorama_);
100 
101  // Set up dynamic reconfiguration
102  ReconfigureServer::CallbackType f = boost::bind(&OptCamNode::config_cb, this, _1, _2);
103  reconfigure_server_.setCallback(f);
104 
105  next_time_ = ros::Time::now();
106  count_ = 0;
107  }
108 
110  {
111  }
112 
114  {
115  IplImage *frame = NULL;
116  ros::Time now;
117 
118  if ( ( frame = camera_->queryFrame() ) == NULL ) {
119  ROS_ERROR_STREAM("[" << serial_id_ << "] cvQueryFrame");
120  return false;
121  }
122  now = ros::Time::now();
123  img_.header.stamp = now;
124  img_omni_.header.stamp = now;
125  img_wide_.header.stamp = now;
126  img_middle_.header.stamp = now;
127  img_narrow_.header.stamp = now;
128  img_panorama_.header.stamp = now;
129  info_.header.stamp = now;
130  info_omni_.header.stamp = now;
131  info_wide_.header.stamp = now;
132  info_middle_.header.stamp = now;
133  info_narrow_.header.stamp = now;
134  info_panorama_.header.stamp = now;
135 
136  try {
137  fillImage(img_, "bgr8", frame->height, frame->width, frame->width * frame->nChannels, frame->imageData);
138  img_pub_.publish(img_, info_);
139  {
140  IplImage *tmp_img_ = camera_->queryOmniFrame();
141  fillImage(img_omni_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
142  img_omni_pub_.publish(img_omni_, info_omni_);
143  }
144  {
145  IplImage *tmp_img_ = camera_->queryWideFrame();
146  fillImage(img_wide_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
147  img_wide_pub_.publish(img_wide_, info_wide_);
148  }
149  {
150  IplImage *tmp_img_ = camera_->queryMiddleFrame();
151  fillImage(img_middle_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
152  img_middle_pub_.publish(img_middle_, info_middle_);
153  }
154  {
155  IplImage *tmp_img_ = camera_->queryNarrowFrame();
156  fillImage(img_narrow_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
157  img_narrow_pub_.publish(img_narrow_, info_narrow_);
158  }
159  if(config_.mode == 2) { // mode = panorama(2)
160  IplImage* tmp_img_ = cvCreateImage(cvSize(frame->width*2,frame->height/2),frame->depth,frame->nChannels);
161  // lower -> left, upper -> right
162  if ( 0 < pan && pan < 180 ) {
163  int offset = frame->width * pan / 180;
164  cvSetImageROI(frame,cvRect(0,frame->height/2,frame->width,frame->height/2));
165  cvSetImageROI(tmp_img_,cvRect(offset,0,frame->width,frame->height/2));
166  cvCopy(frame,tmp_img_);
167  cvSetImageROI(frame,cvRect(frame->width-offset,0,offset,frame->height/2));
168  cvSetImageROI(tmp_img_,cvRect(0,0,offset,frame->height/2));
169  cvCopy(frame,tmp_img_);
170  cvSetImageROI(frame,cvRect(0,0,frame->width-offset,frame->height/2));
171  cvSetImageROI(tmp_img_,cvRect(frame->width+offset,0,frame->width-offset,frame->height/2));
172  cvCopy(frame,tmp_img_);
173  } else if ( -180 < pan && pan < 0 ) {
174  int offset = frame->width * pan / -180;
175  cvSetImageROI(frame,cvRect(0,0,frame->width,frame->height/2));
176  cvSetImageROI(tmp_img_,cvRect(frame->width-offset,0,frame->width,frame->height/2));
177  cvCopy(frame,tmp_img_);
178  cvSetImageROI(frame,cvRect(offset,frame->height/2,frame->width-offset,frame->height/2));
179  cvSetImageROI(tmp_img_,cvRect(0,0,frame->width-offset,frame->height/2));
180  cvCopy(frame,tmp_img_);
181  cvSetImageROI(frame,cvRect(0,frame->height/2,offset,frame->height/2));
182  cvSetImageROI(tmp_img_,cvRect(frame->width*2-offset,0,offset,frame->height/2));
183  cvCopy(frame,tmp_img_);
184  } else { // ( pan == 0 )
185  cvSetImageROI(frame,cvRect(0,frame->height/2,frame->width,frame->height/2));
186  cvSetImageROI(tmp_img_,cvRect(0,0,frame->width,frame->height/2));
187  cvCopy(frame,tmp_img_);
188  cvSetImageROI(frame,cvRect(0,0,frame->width,frame->height/2));
189  cvSetImageROI(tmp_img_,cvRect(frame->width,0,frame->width,frame->height/2));
190  cvCopy(frame,tmp_img_);
191  }
192  fillImage(img_panorama_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
193  img_panorama_pub_.publish(img_panorama_, info_panorama_);
194  cvResetImageROI(frame);
195  cvReleaseImage(&tmp_img_);
196  }
197  }
198  catch (cv_bridge::Exception& e)
199  {
200  ROS_ERROR("Could not convert from opencv to message.");
201  }
202  return true;
203  }
204 
205  void config_cb(opt_camera::OptNM33CameraConfig &config, uint32_t level)
206  {
207 #define SET_CAMERA(methodname, paramname) \
208  if (config_.paramname != config.paramname ) { \
209  camera_->methodname(config.paramname); \
210  }
211 
212  SET_CAMERA(setMode, mode);
213  if ( config.autoexposure ) {
214  SET_CAMERA(setAutoExposure, autoexposure);
215  } else {
216  SET_CAMERA(setExposure, exposure);
217  SET_CAMERA(setIris, iris);
218  }
219  if ( config.mode != 4 && config.mode != 0 ) {
220  pan = config.pan;
221  SET_CAMERA(setPanAbsolute, pan);
222  SET_CAMERA(setTiltAbsolute, tilt);
223  SET_CAMERA(setRollAbsolute, roll);
224  SET_CAMERA(setZoomAbsolute, zoom);
225  }
226  if ( config.autoexposure ) {
227  SET_CAMERA(setBrightness, brightness);
228  }
229  SET_CAMERA(setSharpness, sharpness);
230  if ( ! config.autowhitebalance ) {
231  SET_CAMERA(setWhitebalance, whitebalance);
232  } else {
233  SET_CAMERA(setAutoWhitebalance, autowhitebalance);
234  }
235  config.firmwareversion = camera_->getFirmwareVersion();
236  SET_CAMERA(setFlipScreen, flipscreen);
237  SET_CAMERA(setSmallHemisphere, smallhemisphere);
238  SET_CAMERA(setMedianFilter, medianfilter); // ???
239  SET_CAMERA(setJpegQuality, jpegquality);
240  config.serialid = serial_id_;
241  SET_CAMERA(setInfoDisplay, infodisplay);
242  if ( config.capturefps != config_.capturefps) {
243  SET_CAMERA(setCaptureFPS, capturefps);
244  }
245  //config.actualfps = camera_->getActualFPS();
246  if ( config.lenstype != config_.lenstype) {
247  SET_CAMERA(setLensType, lenstype);
248  }
249  if ( config.mode == 4 ) {
250  // right top
251  //camera_->setPanAbsolute (1,config.camera1_pan);
252  //camera_->setTiltAbsolute(1,config.camera1_tilt);
253  //camera_->setRollAbsolute(1,config.camera1_roll);
254  //camera_->setZoomAbsolute(1,config.camera1_zoom);
255  if ( ( config.camera1_pan != config_.camera1_pan ) ||
256  ( config.camera1_tilt != config_.camera1_tilt ) ||
257  ( config.camera1_roll != config_.camera1_roll ) ||
258  ( config.camera1_zoom != config_.camera1_zoom ) ) {
259  camera_->setLocationAbsolute(1,config.camera1_pan, config.camera1_tilt, config.camera1_roll, config.camera1_zoom);
260  }
261 
262  // left bottom
263  //camera_->setPanAbsolute (2,config.camera2_pan);
264  //camera_->setTiltAbsolute(2,config.camera2_tilt);
265  //camera_->setRollAbsolute(2,config.camera2_roll);
266  //camera_->setZoomAbsolute(2,config.camera2_zoom);w
267  if ( ( config.camera2_pan != config_.camera2_pan ) ||
268  ( config.camera2_tilt != config_.camera2_tilt ) ||
269  ( config.camera2_roll != config_.camera2_roll ) ||
270  ( config.camera2_zoom != config_.camera2_zoom ) ) {
271  camera_->setLocationAbsolute(2,config.camera2_pan, config.camera2_tilt, config.camera2_roll, config.camera2_zoom);
272  }
273 
274  // right bottom
275  if ( ( config.camera3_pan != config_.camera3_pan ) ||
276  ( config.camera3_tilt != config_.camera3_tilt ) ||
277  ( config.camera3_roll != config_.camera3_roll ) ||
278  ( config.camera3_zoom != config_.camera3_zoom ) ) {
279  camera_->setLocationAbsolute(0,config.camera3_pan, config.camera3_tilt, config.camera3_roll, config.camera3_zoom);
280  }
281  //SET_CAMERA(setPanAbsolute, camera3_pan);
282  //SET_CAMERA(setTiltAbsolute, camera3_tilt);
283  //SET_CAMERA(setRollAbsolute, camera3_roll);
284  //SET_CAMERA(setZoomAbsolute, camera3_zoom);
285 #if 0
286  if ( config.camera3_pan != config_.camera3_pan ) {
287  camera_->setPanAbsolute (0,config.camera3_pan);
288  }
289  if ( config.camera3_tilt != config_.camera3_tilt ) {
290  camera_->setTiltAbsolute(0,config.camera3_tilt);
291  }
292  if ( config.camera3_roll != config_.camera3_roll ) {
293  camera_->setRollAbsolute(0,config.camera3_roll);
294  }
295  if ( config.camera3_zoom != config_.camera3_zoom ) {
296  camera_->setZoomAbsolute(0,config.camera3_zoom);
297  }
298 #endif
299  }
300  config_ = config;
301  }
302 
303  bool get_camera_info_method(std::string view_name, sensor_msgs::CameraInfo &info) {
304  std::string cam_name = serial_id_+view_name;
305  std::string ini_name = cam_name+".ini";
306  try {
307 #ifdef ROSPACK_EXPORT
308  rospack::ROSPack rp;
309  rospack::Package *p = rp.get_pkg("opt_camera");
310  if (p!=NULL) ini_name = p->path + "/cfg/" + ini_name;
311 #else
312  rospack::Rospack rp;
313  std::vector<std::string> search_path;
314  rp.getSearchPathFromEnv(search_path);
315  rp.crawl(search_path, 1);
316  std::string path;
317  if (rp.find("opt_camera",path)==true) ini_name = path + "/cfg" + ini_name;
318 #endif
319  } catch (std::runtime_error &e) {
320  }
321  if (!camera_calibration_parsers::readCalibration(ini_name, cam_name, info)) {
322  return false;
323  }
324  if ( info.P[0] == 0.0 ) {
325  ROS_ERROR_STREAM( "Loading wrong camera_info from " << ini_name );
326  return false;
327  }
328  ROS_ERROR_STREAM( "Loading camera_info from " << ini_name << info.P[0] );
329  return true;
330  }
331 
332 #ifdef ROSPACK_EXPORT
333 #define set_camera_info_method(set_camera_info_method, view_name) \
334  bool set_camera_info_method(sensor_msgs::SetCameraInfo::Request& req, \
335  sensor_msgs::SetCameraInfo::Response& rsp) { \
336  ROS_INFO("New camera info received"); \
337  sensor_msgs::CameraInfo &info = req.camera_info; \
338  \
339  std::string cam_name = serial_id_+view_name; \
340  std::string ini_name = cam_name+".ini"; \
341  rospack::ROSPack rp; \
342  try { \
343  rospack::Package *p = rp.get_pkg("opt_camera"); \
344  if (p!=NULL) ini_name = p->path + "/cfg/" + ini_name; \
345  } catch (std::runtime_error &e) { \
346  } \
347  if (!camera_calibration_parsers::writeCalibration(ini_name, cam_name.c_str(), info)) { \
348  rsp.status_message = "Error writing camera_info to " + cam_name + ".ini"; \
349  rsp.success = false; \
350  } \
351  \
352  rsp.success = true; \
353  return true; \
354  }
355 #else
356 #define set_camera_info_method(set_camera_info_method, view_name) \
357  bool set_camera_info_method(sensor_msgs::SetCameraInfo::Request& req, \
358  sensor_msgs::SetCameraInfo::Response& rsp) { \
359  ROS_INFO("New camera info received"); \
360  sensor_msgs::CameraInfo &info = req.camera_info; \
361  \
362  std::string cam_name = serial_id_+view_name; \
363  std::string ini_name = cam_name+".ini"; \
364  rospack::Rospack rp; \
365  try { \
366  std::vector<std::string> search_path; \
367  rp.getSearchPathFromEnv(search_path); \
368  rp.crawl(search_path, 1); \
369  std::string path; \
370  if(rp.find("opt_camera", path)==true) ini_name = path + "/cfg/" + ini_name; \
371  } catch (std::runtime_error &e) { \
372  } \
373  if (!camera_calibration_parsers::writeCalibration(ini_name, cam_name.c_str(), info)) { \
374  rsp.status_message = "Error writing camera_info to " + cam_name + ".ini"; \
375  rsp.success = false; \
376  } \
377  \
378  rsp.success = true; \
379  return true; \
380  }
381 #endif
382 
383  set_camera_info_method(set_camera_info,"")
384  set_camera_info_method(set_camera_info_omni,"-omni")
385  set_camera_info_method(set_camera_info_wide,"-wide")
386  set_camera_info_method(set_camera_info_middle,"-middle")
387  set_camera_info_method(set_camera_info_narrow,"-narrow")
388  set_camera_info_method(set_camera_info_panorama,"-panorama")
389 
390  bool spin()
391  {
392  while (node_.ok())
393  {
394  if (take_and_send_image())
395  {
396  count_++;
397  ros::Time now_time = ros::Time::now();
398  if (now_time > next_time_) {
399  std::cout << count_ << " frames/sec at " << now_time << std::endl;
400  count_ = 0;
401  next_time_ = next_time_ + ros::Duration(1,0);
402  }
403  } else {
404  ROS_ERROR_STREAM("[" << node_.getNamespace() << "] couldn't take image.");
405  usleep(1000000);
406  }
407  ros::spinOnce();
408  }
409  return true;
410  }
411 
412 };
413 
414 
415 int main (int argc, char **argv)
416 {
417  ros::init(argc, argv, "nm33_cam");
418  ros::NodeHandle nh("camera");
419 
420  OptCamNode camera(nh);
421 
422  camera.spin();
423 
424  return 0;
425 }
std::string getFirmwareVersion()
ros::ServiceServer info_wide_srv_
Definition: nm33_node.cpp:31
bool setTiltAbsolute(double value)
bool setRollAbsolute(double value)
ros::ServiceServer info_narrow_srv_
Definition: nm33_node.cpp:31
IplImage * queryFrame()
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
IplImage * queryOmniFrame()
sensor_msgs::Image img_panorama_
Definition: nm33_node.cpp:28
def readCalibration(file_name)
image_transport::CameraPublisher img_panorama_pub_
Definition: nm33_node.cpp:29
IplImage * queryWideFrame()
std::string serial_id_
Definition: nm33_node.cpp:20
f
ros::ServiceServer info_middle_srv_
Definition: nm33_node.cpp:31
image_transport::CameraPublisher img_narrow_pub_
Definition: nm33_node.cpp:29
image_transport::CameraPublisher img_pub_
Definition: nm33_node.cpp:29
sensor_msgs::CameraInfo info_middle_
Definition: nm33_node.cpp:30
sensor_msgs::CameraInfo info_narrow_
Definition: nm33_node.cpp:30
sensor_msgs::CameraInfo info_wide_
Definition: nm33_node.cpp:30
dynamic_reconfigure::Server< opt_camera::OptNM33CameraConfig > ReconfigureServer
Definition: nm33_node.cpp:34
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
sensor_msgs::CameraInfo info_
Definition: nm33_node.cpp:30
sensor_msgs::CameraInfo info_omni_
Definition: nm33_node.cpp:30
std::string getSerialID()
IplImage * queryNarrowFrame()
opt_camera::OptNM33CameraConfig config_
Definition: nm33_node.cpp:36
sensor_msgs::Image img_wide_
Definition: nm33_node.cpp:28
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
sensor_msgs::Image img_narrow_
Definition: nm33_node.cpp:28
image_transport::CameraPublisher img_omni_pub_
Definition: nm33_node.cpp:29
OptCamNode(ros::NodeHandle &node)
Definition: nm33_node.cpp:40
ros::Time next_time_
Definition: nm33_node.cpp:24
image_transport::CameraPublisher img_wide_pub_
Definition: nm33_node.cpp:29
omni middle panorama bool spin()
Definition: nm33_node.cpp:390
ros::NodeHandle & node_
Definition: nm33_node.cpp:23
OptNM3xCamera * camera_
Definition: nm33_node.cpp:19
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
sensor_msgs::Image img_middle_
Definition: nm33_node.cpp:28
sensor_msgs::Image img_
Definition: nm33_node.cpp:28
bool setZoomAbsolute(double value)
ros::ServiceServer info_srv_
Definition: nm33_node.cpp:31
#define SET_CAMERA(methodname, paramname)
bool setPanAbsolute(double value)
const std::string & getNamespace() const
ros::ServiceServer info_omni_srv_
Definition: nm33_node.cpp:31
bool getSearchPathFromEnv(std::vector< std::string > &sp)
set_camera_info_method(set_camera_info,"") set_camera_info_method(set_camera_info_omni
bool take_and_send_image()
Definition: nm33_node.cpp:113
void config_cb(opt_camera::OptNM33CameraConfig &config, uint32_t level)
Definition: nm33_node.cpp:205
bool setLocationAbsolute(int no, int pan, int tilt, int roll, int zoom)
sensor_msgs::CameraInfo info_panorama_
Definition: nm33_node.cpp:30
void crawl(std::vector< std::string > search_path, bool force)
ros::ServiceServer info_panorama_srv_
Definition: nm33_node.cpp:31
bool getParam(const std::string &key, std::string &s) const
bool setSmallHemisphere(char value)
bool find(const std::string &name, std::string &path)
static Time now()
sensor_msgs::Image img_omni_
Definition: nm33_node.cpp:28
int main(int argc, char **argv)
Definition: nm33_node.cpp:415
image_transport::CameraPublisher img_middle_pub_
Definition: nm33_node.cpp:29
bool ok() const
ReconfigureServer reconfigure_server_
Definition: nm33_node.cpp:35
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
bool get_camera_info_method(std::string view_name, sensor_msgs::CameraInfo &info)
Definition: nm33_node.cpp:303
IplImage * queryMiddleFrame()


opt_camera
Author(s): Kei Okada
autogenerated on Wed Jul 10 2019 03:47:11