driver.cpp
Go to the documentation of this file.
00001 #include "cv_camera/driver.h"
00002 
00003 namespace
00004 {
00005 const double DEFAULT_RATE = 30.0;
00006 const int32_t PUBLISHER_BUFFER_SIZE = 1;
00007 }
00008 
00009 namespace cv_camera
00010 {
00011 
00012 Driver::Driver(ros::NodeHandle& private_node,
00013                ros::NodeHandle& camera_node) :
00014     private_node_(private_node),
00015     camera_node_(camera_node)
00016 {
00017 }
00018 
00019 void Driver::setup() {
00020   double hz(DEFAULT_RATE);
00021   int32_t device_id(0);
00022   std::string frame_id("camera");
00023   std::string file_path("");
00024   
00025   private_node_.getParam("device_id", device_id);
00026   private_node_.getParam("frame_id", frame_id);
00027   private_node_.getParam("rate", hz);
00028 
00029   int32_t image_width(640);
00030   int32_t image_height(480);
00031   
00032   camera_.reset(new Capture(camera_node_,
00033                             "image_raw",
00034                             PUBLISHER_BUFFER_SIZE,
00035                             frame_id));
00036   if (private_node_.getParam("file", file_path) &&
00037       file_path != "") {
00038     camera_->openFile(file_path);
00039   } else {
00040     camera_->open(device_id);
00041   }
00042   if (private_node_.getParam("image_width", image_width)) {
00043     if(!camera_->setWidth(image_width)) {
00044       ROS_WARN("fail to set image_width");
00045     }
00046   }
00047   if (private_node_.getParam("image_height", image_height)) {
00048     if(!camera_->setHeight(image_height)) {
00049       ROS_WARN("fail to set image_height");
00050     }
00051   }
00052   
00053   rate_.reset(new ros::Rate(hz));
00054 }
00055 
00056 void Driver::proceed() {
00057   if (camera_->capture()) {
00058     camera_->publish();
00059   }
00060   rate_->sleep();
00061 }
00062 
00063 Driver::~Driver() {
00064 }
00065 
00066 }  // namespace cv_camera


cv_camera
Author(s): Takashi Ogura
autogenerated on Wed Aug 26 2015 11:13:42