usb_cam_node.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2014, Robert Bosch LLC.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Robert Bosch nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 *********************************************************************/
36 
37 #include <ros/ros.h>
38 #include <usb_cam/usb_cam.h>
41 #include <sstream>
42 #include <std_srvs/Empty.h>
43 
44 namespace usb_cam {
45 
47 {
48 public:
49  // private ROS node handle
51 
52  // shared image message
53  sensor_msgs::Image img_;
55 
56  // parameters
58  //std::string start_service_name_, start_service_name_;
64 
66 
68 
69 
70 
71  bool service_start_cap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res )
72  {
73  cam_.start_capturing();
74  return true;
75  }
76 
77 
78  bool service_stop_cap( std_srvs::Empty::Request &req, std_srvs::Empty::Response &res )
79  {
80  cam_.stop_capturing();
81  return true;
82  }
83 
85  node_("~")
86  {
87  // advertise the main image topic
89  image_pub_ = it.advertiseCamera("image_raw", 1);
90 
91  // grab the parameters
92  node_.param("video_device", video_device_name_, std::string("/dev/video0"));
93  node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone"
94  node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone"
95  node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone"
96  node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone"
97  // possible values: mmap, read, userptr
98  node_.param("io_method", io_method_name_, std::string("mmap"));
99  node_.param("image_width", image_width_, 640);
100  node_.param("image_height", image_height_, 480);
101  node_.param("framerate", framerate_, 30);
102  // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
103  node_.param("pixel_format", pixel_format_name_, std::string("mjpeg"));
104  // enable/disable autofocus
105  node_.param("autofocus", autofocus_, false);
106  node_.param("focus", focus_, -1); //0-255, -1 "leave alone"
107  // enable/disable autoexposure
108  node_.param("autoexposure", autoexposure_, true);
109  node_.param("exposure", exposure_, 100);
110  node_.param("gain", gain_, -1); //0-100?, -1 "leave alone"
111  // enable/disable auto white balance temperature
112  node_.param("auto_white_balance", auto_white_balance_, true);
113  node_.param("white_balance", white_balance_, 4000);
114 
115  // load the camera info
116  node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
117  node_.param("camera_name", camera_name_, std::string("head_camera"));
118  node_.param("camera_info_url", camera_info_url_, std::string(""));
119  cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
120 
121  // create Services
122  service_start_ = node_.advertiseService("start_capture", &UsbCamNode::service_start_cap, this);
123  service_stop_ = node_.advertiseService("stop_capture", &UsbCamNode::service_stop_cap, this);
124 
125  // check for default camera info
126  if (!cinfo_->isCalibrated())
127  {
128  cinfo_->setCameraName(video_device_name_);
129  sensor_msgs::CameraInfo camera_info;
130  camera_info.header.frame_id = img_.header.frame_id;
131  camera_info.width = image_width_;
132  camera_info.height = image_height_;
133  cinfo_->setCameraInfo(camera_info);
134  }
135 
136 
137  ROS_INFO("Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", camera_name_.c_str(), video_device_name_.c_str(),
138  image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), framerate_);
139 
140  // set the IO method
141  UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
142  if(io_method == UsbCam::IO_METHOD_UNKNOWN)
143  {
144  ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
145  node_.shutdown();
146  return;
147  }
148 
149  // set the pixel format
150  UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_);
151  if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
152  {
153  ROS_FATAL("Unknown pixel format '%s'", pixel_format_name_.c_str());
154  node_.shutdown();
155  return;
156  }
157 
158  // start the camera
159  cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_,
161 
162  // set camera parameters
163  if (brightness_ >= 0)
164  {
165  cam_.set_v4l_parameter("brightness", brightness_);
166  }
167 
168  if (contrast_ >= 0)
169  {
170  cam_.set_v4l_parameter("contrast", contrast_);
171  }
172 
173  if (saturation_ >= 0)
174  {
175  cam_.set_v4l_parameter("saturation", saturation_);
176  }
177 
178  if (sharpness_ >= 0)
179  {
180  cam_.set_v4l_parameter("sharpness", sharpness_);
181  }
182 
183  if (gain_ >= 0)
184  {
185  cam_.set_v4l_parameter("gain", gain_);
186  }
187 
188  // check auto white balance
189  if (auto_white_balance_)
190  {
191  cam_.set_v4l_parameter("white_balance_temperature_auto", 1);
192  }
193  else
194  {
195  cam_.set_v4l_parameter("white_balance_temperature_auto", 0);
196  cam_.set_v4l_parameter("white_balance_temperature", white_balance_);
197  }
198 
199  // check auto exposure
200  if (!autoexposure_)
201  {
202  // turn down exposure control (from max of 3)
203  cam_.set_v4l_parameter("exposure_auto", 1);
204  // change the exposure level
205  cam_.set_v4l_parameter("exposure_absolute", exposure_);
206  }
207 
208  // check auto focus
209  if (autofocus_)
210  {
211  cam_.set_auto_focus(1);
212  cam_.set_v4l_parameter("focus_auto", 1);
213  }
214  else
215  {
216  cam_.set_v4l_parameter("focus_auto", 0);
217  if (focus_ >= 0)
218  {
219  cam_.set_v4l_parameter("focus_absolute", focus_);
220  }
221  }
222  }
223 
224  virtual ~UsbCamNode()
225  {
226  cam_.shutdown();
227  }
228 
230  {
231  // grab the image
232  cam_.grab_image(&img_);
233 
234  // grab the camera info
235  sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
236  ci->header.frame_id = img_.header.frame_id;
237  ci->header.stamp = img_.header.stamp;
238 
239  // publish the image
240  image_pub_.publish(img_, *ci);
241 
242  return true;
243  }
244 
245  bool spin()
246  {
247  ros::Rate loop_rate(this->framerate_);
248  while (node_.ok())
249  {
250  if (cam_.is_capturing()) {
251  if (!take_and_send_image()) ROS_WARN("USB camera did not respond in time.");
252  }
253  ros::spinOnce();
254  loop_rate.sleep();
255 
256  }
257  return true;
258  }
259 
260 
261 
262 
263 
264 
265 };
266 
267 }
268 
269 int main(int argc, char **argv)
270 {
271  ros::init(argc, argv, "usb_cam");
273  a.spin();
274  return EXIT_SUCCESS;
275 }
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
#define ROS_FATAL(...)
std::string pixel_format_name_
int main(int argc, char **argv)
void start_capturing(void)
Definition: usb_cam.cpp:616
void set_auto_focus(int value)
Definition: usb_cam.cpp:1135
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void shutdown(void)
Definition: usb_cam.cpp:1059
ros::NodeHandle node_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
#define ROS_WARN(...)
void stop_capturing(void)
Definition: usb_cam.cpp:592
ros::ServiceServer service_stop_
static pixel_format pixel_format_from_string(const std::string &str)
Definition: usb_cam.cpp:1229
sensor_msgs::Image img_
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
std::string io_method_name_
bool service_start_cap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
std::string video_device_name_
image_transport::CameraPublisher image_pub_
bool is_capturing()
Definition: usb_cam.cpp:588
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::ServiceServer service_start_
std::string camera_info_url_
void set_v4l_parameter(const std::string &param, int value)
Definition: usb_cam.cpp:1181
static io_method io_method_from_string(const std::string &str)
Definition: usb_cam.cpp:1217
std::string camera_name_
bool sleep()
bool service_stop_cap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
boost::shared_ptr< camera_info_manager::CameraInfoManager > cinfo_
bool ok() const
void grab_image(sensor_msgs::Image *image)
Definition: usb_cam.cpp:1082
ROSCPP_DECL void spinOnce()
void start(const std::string &dev, io_method io, pixel_format pf, int image_width, int image_height, int framerate)
Definition: usb_cam.cpp:1005


usb_cam
Author(s): Benjamin Pitzer
autogenerated on Fri Jun 7 2019 21:46:25