From 966ad3d38a2bd4be41416757fd52cad9e7dbe79c Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sun, 2 Jul 2017 05:44:32 -0700 Subject: [PATCH] use the v4l2_buffer timestamp if available. #75 usb_cam.cpp is building but untested #103 Builds but crashes immediately after running data is getting published, no image shown image shown, frame rate is very slow #103 --- CHANGELOG.rst | 5 + CMakeLists.txt | 60 +++--- README.md | 4 +- include/usb_cam/usb_cam.h | 60 +++--- nodes/usb_cam_node.cpp | 132 ++++++++----- package.xml | 46 +++-- scripts/show_image.py | 61 ++++++ src/usb_cam.cpp | 383 ++++++++++++++++++++++++++------------ 8 files changed, 505 insertions(+), 246 deletions(-) create mode 100755 scripts/show_image.py diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4e763df7..cdf7e6a6 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package usb_cam ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.7 (2018-10-29) +------------------ + +* ROS2 version + 0.3.6 (2017-06-15) ------------------ * .travis.yml: udpate to trusty diff --git a/CMakeLists.txt b/CMakeLists.txt index be662c4d..4a03265b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,34 +1,24 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(usb_cam) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS image_transport roscpp std_msgs std_srvs sensor_msgs camera_info_manager) + +# find_package(camera_info_manager REQUIRED) +# find_package(image_transport REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) ## pkg-config libraries find_package(PkgConfig REQUIRED) pkg_check_modules(avcodec libavcodec REQUIRED) pkg_check_modules(swscale libswscale REQUIRED) -################################################### -## Declare things to be passed to other projects ## -################################################### - -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) - -########### -## Build ## -########### - include_directories(include - ${catkin_INCLUDE_DIRS} ${avcodec_INCLUDE_DIRS} ${swscale_INCLUDE_DIRS} ) @@ -38,7 +28,6 @@ add_library(${PROJECT_NAME} src/usb_cam.cpp) target_link_libraries(${PROJECT_NAME} ${avcodec_LIBRARIES} ${swscale_LIBRARIES} - ${catkin_LIBRARIES} ) ## Declare a cpp executable @@ -47,26 +36,21 @@ target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${avcodec_LIBRARIES} ${swscale_LIBRARIES} - ${catkin_LIBRARIES} + # TODO(lucasw) should this have been in libavcodec? + avutil ) -############# -## Install ## -############# - -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +ament_target_dependencies(${PROJECT_NAME} + "builtin_interfaces" + "rclcpp" + "sensor_msgs" + "std_msgs" + "std_srvs" ) -## Copy launch files -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - FILES_MATCHING PATTERN "*.launch" -) +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS scripts/show_image.py DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/README.md b/README.md index ff5d68b0..fe6d8a52 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,10 @@ usb_cam [![Build Status](https://api.travis-ci.org/bosch-ros-pkg/usb_cam.png)](https://travis-ci.org/bosch-ros-pkg/usb_cam) ======= -#### A ROS Driver for V4L USB Cameras +#### A ROS2 Driver for V4L USB Cameras This package is based off of V4L devices specifically instead of just UVC. -For full documentation, see [the ROS wiki](http://ros.org/wiki/usb_cam). +For ros1 documentation, see [the ROS wiki](http://ros.org/wiki/usb_cam). [Doxygen](http://docs.ros.org/indigo/api/usb_cam/html/) files can be found on the ROS wiki. diff --git a/include/usb_cam/usb_cam.h b/include/usb_cam/usb_cam.h index 2aa36061..10e24cce 100644 --- a/include/usb_cam/usb_cam.h +++ b/include/usb_cam/usb_cam.h @@ -52,10 +52,23 @@ extern "C" #define AV_CODEC_ID_MJPEG CODEC_ID_MJPEG #endif +#include +#include +// #include #include #include -#include +#define ROS_INFO(msg, ...) printf(msg, ##__VA_ARGS__) +#define ROS_ERROR(msg, ...) printf(msg, ##__VA_ARGS__) +#define ROS_WARN(msg, ...) printf(msg, ##__VA_ARGS__) +#define ROS_DEBUG(msg, ...) printf(msg, ##__VA_ARGS__) +// #define ROS_DEBUG(msg, ...) // ##__VA_ARGS__ +#define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) + +#define INFO(msg) std::cout << "I [" << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << "] " << msg << "\n" +#define WARN(msg) std::cout << "W [" << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << "] " << msg << "\n" +#define ERROR(msg) std::cerr << "E [" << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << "] " << msg << std::endl +#define ROS_ERROR_STREAM(msg) ERROR(msg) namespace usb_cam { @@ -75,36 +88,40 @@ class UsbCam { ~UsbCam(); // start camera - void start(const std::string& dev, io_method io, pixel_format pf, + bool start(const std::string& dev, io_method io, pixel_format pf, int image_width, int image_height, int framerate); // shutdown camera - void shutdown(void); + bool shutdown(void); // grabs a new image from the camera - void grab_image(sensor_msgs::Image* image); + // bool get_image(sensor_msgs::msg::Image:::SharedPtr image); + bool get_image(builtin_interfaces::msg::Time& stamp, + std::string& encoding, uint32_t& height, uint32_t& width, + uint32_t& step, std::vector& data); // enables/disable auto focus - void set_auto_focus(int value); + bool set_auto_focus(int value); // Set video device parameters - void set_v4l_parameter(const std::string& param, int value); - void set_v4l_parameter(const std::string& param, const std::string& value); + bool set_v4l_parameter(const std::string& param, int value); + bool set_v4l_parameter(const std::string& param, const std::string& value); static io_method io_method_from_string(const std::string& str); static pixel_format pixel_format_from_string(const std::string& str); - void stop_capturing(void); - void start_capturing(void); + bool stop_capturing(void); + bool start_capturing(void); bool is_capturing(); private: + // TODO(lucasw) just store an Image shared_ptr here typedef struct { int width; int height; int bytes_per_pixel; int image_size; - ros::Time stamp; + builtin_interfaces::msg::Time stamp; char *image; int is_new; } camera_image_t; @@ -117,20 +134,21 @@ class UsbCam { int init_mjpeg_decoder(int image_width, int image_height); - void mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels); - void process_image(const void * src, int len, camera_image_t *dest); - int read_frame(); - void uninit_device(void); - void init_read(unsigned int buffer_size); - void init_mmap(void); - void init_userp(unsigned int buffer_size); - void init_device(int image_width, int image_height, int framerate); - void close_device(void); - void open_device(void); - void grab_image(); + bool mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels); + bool process_image(const void * src, int len, camera_image_t *dest); + bool read_frame(); + bool uninit_device(void); + bool init_read(unsigned int buffer_size); + bool init_mmap(void); + bool init_userp(unsigned int buffer_size); + bool init_device(int image_width, int image_height, int framerate); + bool close_device(void); + bool open_device(void); + bool grab_image(); bool is_capturing_; + rclcpp::Clock::SharedPtr clock_; std::string camera_dev_; unsigned int pixelformat_; bool monochrome_; diff --git a/nodes/usb_cam_node.cpp b/nodes/usb_cam_node.cpp index 2b4686e0..73b29fec 100644 --- a/nodes/usb_cam_node.cpp +++ b/nodes/usb_cam_node.cpp @@ -34,24 +34,24 @@ * *********************************************************************/ -#include +#include #include -#include -#include +// #include +// #include +#include #include -#include +// #include + +using namespace std::chrono_literals; namespace usb_cam { -class UsbCamNode +class UsbCamNode : public rclcpp::Node { public: - // private ROS node handle - ros::NodeHandle node_; - // shared image message - sensor_msgs::Image img_; - image_transport::CameraPublisher image_pub_; + sensor_msgs::msg::Image::SharedPtr img_; + rclcpp::Publisher::SharedPtr image_pub_; // parameters std::string video_device_name_, io_method_name_, pixel_format_name_, camera_name_, camera_info_url_; @@ -60,13 +60,14 @@ class UsbCamNode int image_width_, image_height_, framerate_, exposure_, brightness_, contrast_, saturation_, sharpness_, focus_, white_balance_, gain_; bool autofocus_, autoexposure_, auto_white_balance_; - boost::shared_ptr cinfo_; + // boost::shared_ptr cinfo_; UsbCam cam_; - ros::ServiceServer service_start_, service_stop_; - + rclcpp::TimerBase::SharedPtr timer_; +#if 0 + ros::ServiceServer service_start_, service_stop_; bool service_start_cap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res ) { @@ -80,27 +81,48 @@ class UsbCamNode cam_.stop_capturing(); return true; } - - UsbCamNode() : - node_("~") +#endif + + UsbCamNode() : Node("usb_cam"), + video_device_name_("/dev/video0"), + io_method_name_("mmap"), + image_width_(960), + image_height_(540), + framerate_(15), + pixel_format_name_("mjpeg") { + img_ = std::make_shared(); // advertise the main image topic - image_transport::ImageTransport it(node_); - image_pub_ = it.advertiseCamera("image_raw", 1); + image_pub_ = this->create_publisher("image_raw"); + +#if 0 + auto parameters_client = std::make_shared(shared_from_this()); // grab the parameters - node_.param("video_device", video_device_name_, std::string("/dev/video0")); + auto dev_param = parameters_client->get_parameter("video_device"); + if (dev_param) + video_device_name_ = dev_param.value_to_string(); +#endif + +#if 0 node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone" node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone" node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone" node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone" +#endif + +#if 0 // possible values: mmap, read, userptr node_.param("io_method", io_method_name_, std::string("mmap")); node_.param("image_width", image_width_, 640); node_.param("image_height", image_height_, 480); node_.param("framerate", framerate_, 30); + node_.param("frame_id", frame_id, 30); // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24 node_.param("pixel_format", pixel_format_name_, std::string("mjpeg")); +#endif + +#if 0 // enable/disable autofocus node_.param("autofocus", autofocus_, false); node_.param("focus", focus_, -1); //0-255, -1 "leave alone" @@ -111,7 +133,10 @@ class UsbCamNode // enable/disable auto white balance temperature node_.param("auto_white_balance", auto_white_balance_, true); node_.param("white_balance", white_balance_, 4000); +#endif + std::string camera_frame_id = "map"; +#if 0 // load the camera info node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera")); node_.param("camera_name", camera_name_, std::string("head_camera")); @@ -132,17 +157,20 @@ class UsbCamNode camera_info.height = image_height_; cinfo_->setCameraInfo(camera_info); } +#endif + img_->header.frame_id = camera_frame_id; - - ROS_INFO("Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", camera_name_.c_str(), video_device_name_.c_str(), - image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), framerate_); + RCLCPP_INFO(this->get_logger(), "Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", + camera_name_.c_str(), video_device_name_.c_str(), + image_width_, image_height_, io_method_name_.c_str(), + pixel_format_name_.c_str(), framerate_); // set the IO method UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_); if(io_method == UsbCam::IO_METHOD_UNKNOWN) { - ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str()); - node_.shutdown(); + RCLCPP_ERROR(this->get_logger(), "Unknown IO method '%s'", io_method_name_.c_str()); + rclcpp::shutdown(); return; } @@ -150,8 +178,8 @@ class UsbCamNode UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_); if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN) { - ROS_FATAL("Unknown pixel format '%s'", pixel_format_name_.c_str()); - node_.shutdown(); + RCLCPP_ERROR(this->get_logger(), "Unknown pixel format '%s'", pixel_format_name_.c_str()); + rclcpp::shutdown(); return; } @@ -159,6 +187,7 @@ class UsbCamNode cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_, image_height_, framerate_); +#if 0 // set camera parameters if (brightness_ >= 0) { @@ -219,18 +248,30 @@ class UsbCamNode cam_.set_v4l_parameter("focus_absolute", focus_); } } +#endif + + // TODO(lucasw) should this check a little faster than expected frame rate? + timer_ = this->create_wall_timer(33ms, + std::bind(&UsbCamNode::update, this)); + INFO("starting timer"); } virtual ~UsbCamNode() { + WARN("shutting down"); cam_.shutdown(); } bool take_and_send_image() { // grab the image - cam_.grab_image(&img_); + if (!cam_.get_image(img_->header.stamp, img_->encoding, img_->height, img_->width, + img_->step, img_->data)) { + ERROR("grab failed"); + return false; + } +#if 0 // grab the camera info sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo())); ci->header.frame_id = img_.header.frame_id; @@ -238,38 +279,35 @@ class UsbCamNode // publish the image image_pub_.publish(img_, *ci); +#endif + // INFO(img_->data.size() << " " << img_->width << " " << img_->height << " " << img_->step); + image_pub_->publish(img_); return true; } - bool spin() + void update() { - ros::Rate loop_rate(this->framerate_); - while (node_.ok()) - { - if (cam_.is_capturing()) { - if (!take_and_send_image()) ROS_WARN("USB camera did not respond in time."); + if (cam_.is_capturing()) { + if (!take_and_send_image()) { + WARN("USB camera did not respond in time."); } - ros::spinOnce(); - loop_rate.sleep(); - } - return true; } - - - - - - }; } int main(int argc, char **argv) { - ros::init(argc, argv, "usb_cam"); - usb_cam::UsbCamNode a; - a.spin(); - return EXIT_SUCCESS; + rclcpp::init(argc, argv); + + // Force flush of the stdout buffer. + // This ensures a correct sync of all prints + // even when executed simultaneously within a launch file. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::spin(std::make_shared()); + INFO("node done"); + rclcpp::shutdown(); + return 0; } diff --git a/package.xml b/package.xml index ab52497b..40da1cb3 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,9 @@ - + + + usb_cam - 0.3.6 - A ROS Driver for V4L USB Cameras + 0.3.7 + A ROS2 Driver for V4L USB Cameras Evan Flynn @@ -10,25 +12,37 @@ BSD http://wiki.ros.org/usb_cam - https://github.com/bosch-ros-pkg/usb_cam/issues - https://github.com/bosch-ros-pkg/usb_cam + https://github.com/ros-drivers/usb_cam/issues + https://github.com/ros-drivers/usb_cam - catkin + ament_cmake - image_transport - roscpp - std_msgs - std_srvs - sensor_msgs + ament_lint_auto + ament_lint_common + + + + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + diff --git a/scripts/show_image.py b/scripts/show_image.py new file mode 100755 index 00000000..474b941e --- /dev/null +++ b/scripts/show_image.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 + +import cv2 +import numpy as np +import rclpy + +from rclpy.node import Node +from sensor_msgs.msg import Image + + +class ExamineImage(Node): + def __init__(self): + super().__init__('examine_image') + + self.mat = None + self.sub = self.create_subscription( + Image, + 'image_raw', + self.image_callback) + # print("subscribed to " + self.sub.getTopic()) + + def image_callback(self, msg): + sz = (msg.height, msg.width) + print(msg.header.stamp) + if False: + print("{encoding} {width} {height} {step} {data_size}".format( + encoding=msg.encoding, width=msg.width, height=msg.height, + step=msg.step, data_size=len(msg.data))) + if msg.step * msg.height != len(msg.data): + print("bad step/height/data size") + return + + if msg.encoding == "rgb8": + dirty = (self.mat is None or msg.width != self.mat.shape[1] or + msg.height != self.mat.shape[0] or len(self.mat.shape) < 2 or + self.mat.shape[2] != 3) + if dirty: + self.mat = np.zeros([msg.height, msg.width, 3], dtype=np.uint8) + self.mat[:, :, 2] = np.array(msg.data[0::3]).reshape(sz) + self.mat[:, :, 1] = np.array(msg.data[1::3]).reshape(sz) + self.mat[:, :, 0] = np.array(msg.data[2::3]).reshape(sz) + elif msg.encoding == "mono8": + self.mat = np.array(msg.data).reshape(sz) + else: + print("unsupported encoding {}".format(msg.encoding)) + return + if self.mat is not None: + cv2.imshow("image", self.mat) + cv2.waitKey(5) + +def main(args=None): + rclpy.init(args=args) + + examine_image = ExamineImage() + rclpy.spin(examine_image) + + examine_image.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index d34598b7..6fd696dd 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -39,29 +39,57 @@ #include #include #include /* low-level i/o */ +#include #include #include #include +#include #include #include #include #include #include -#include #include -#include +// #include #include #define CLEAR(x) memset (&(x), 0, sizeof (x)) + namespace usb_cam { -static void errno_exit(const char * s) +void monotonicToRealTime(const timespec& monotonic_time, timespec& real_time) { - ROS_ERROR("%s error %d, %s", s, errno, strerror(errno)); - exit(EXIT_FAILURE); + struct timespec real_sample1, real_sample2, monotonic_sample; + + // TODO(lucasw) Disable interrupts here? + // otherwise what if there is a delay/interruption between sampling the times? + clock_gettime(CLOCK_REALTIME, &real_sample1); + clock_gettime(CLOCK_MONOTONIC, &monotonic_sample); + clock_gettime(CLOCK_REALTIME, &real_sample2); + + timespec time_diff; + time_diff.tv_sec = real_sample2.tv_sec - monotonic_sample.tv_sec; + time_diff.tv_nsec = real_sample2.tv_nsec - monotonic_sample.tv_nsec; + + // This isn't available outside of the kernel + // real_time = timespec_add(monotonic_time, time_diff); + + const long NSEC_PER_SEC = 1000000000; + real_time.tv_sec = monotonic_time.tv_sec + time_diff.tv_sec; + real_time.tv_nsec = monotonic_time.tv_nsec + time_diff.tv_nsec; + if (real_time.tv_nsec >= NSEC_PER_SEC) + { + ++real_time.tv_sec; + real_time.tv_nsec -= NSEC_PER_SEC; + } + else if (real_time.tv_nsec < 0) + { + --real_time.tv_sec; + real_time.tv_nsec += NSEC_PER_SEC; + } } static int xioctl(int fd, int request, void * arg) @@ -267,7 +295,7 @@ static unsigned char CLIPVALUE(int val) * [ B ] [ 1.0 2.041 0.002 ] [ V ] * */ -static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned char v, unsigned char* r, +static bool YUV2RGB(const unsigned char y, const unsigned char u, const unsigned char v, unsigned char* r, unsigned char* g, unsigned char* b) { const int y2 = (int)y; @@ -290,9 +318,11 @@ static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned *r = CLIPVALUE(r2); *g = CLIPVALUE(g2); *b = CLIPVALUE(b2); + + return true; } -void uyvy2rgb(char *YUV, char *RGB, int NumPixels) +bool uyvy2rgb(char *YUV, char *RGB, int NumPixels) { int i, j; unsigned char y0, y1, u, v; @@ -312,9 +342,10 @@ void uyvy2rgb(char *YUV, char *RGB, int NumPixels) RGB[j + 4] = g; RGB[j + 5] = b; } + return true; } -static void mono102mono8(char *RAW, char *MONO, int NumPixels) +static bool mono102mono8(char *RAW, char *MONO, int NumPixels) { int i, j; for (i = 0, j = 0; i < (NumPixels << 1); i += 2, j += 1) @@ -322,9 +353,10 @@ static void mono102mono8(char *RAW, char *MONO, int NumPixels) //first byte is low byte, second byte is high byte; smash together and convert to 8-bit MONO[j] = (unsigned char)(((RAW[i + 0] >> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0)); } + return true; } -static void yuyv2rgb(char *YUV, char *RGB, int NumPixels) +static bool yuyv2rgb(char *YUV, char *RGB, int NumPixels) { int i, j; unsigned char y0, y1, u, v; @@ -345,6 +377,7 @@ static void yuyv2rgb(char *YUV, char *RGB, int NumPixels) RGB[j + 4] = g; RGB[j + 5] = b; } + return true; } void rgb242rgb(char *YUV, char *RGB, int NumPixels) @@ -357,6 +390,7 @@ UsbCam::UsbCam() : io_(IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), avframe_camera_(NULL), avframe_rgb_(NULL), avcodec_(NULL), avoptions_(NULL), avcodec_context_(NULL), avframe_camera_size_(0), avframe_rgb_size_(0), video_sws_(NULL), image_(NULL), is_capturing_(false) { + clock_ = std::make_shared(RCL_SYSTEM_TIME); } UsbCam::~UsbCam() { @@ -390,7 +424,9 @@ int UsbCam::init_mjpeg_decoder(int image_width, int image_height) avcodec_context_->height = image_height; #if LIBAVCODEC_VERSION_MAJOR > 52 + // TODO(lucasw) it gets set correctly here, but then changed later to deprecated J422P format avcodec_context_->pix_fmt = AV_PIX_FMT_YUV422P; + INFO("using YUV422P " << AV_PIX_FMT_YUV422P << " " << avcodec_context_->pix_fmt); avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO; #endif @@ -403,13 +439,17 @@ int UsbCam::init_mjpeg_decoder(int image_width, int image_height) ROS_ERROR("Could not open MJPEG Decoder"); return 0; } + INFO("pixel format " << AV_PIX_FMT_YUV422P << " " << avcodec_context_->pix_fmt); return 1; } -void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels) +bool UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels) { + // INFO("mjpeg2rgb " << len << ", image 0x" << std::hex << (unsigned long int)RGB << std::dec << " " << NumPixels + // << ", avframe_rgb_size_ " << avframe_rgb_size_); int got_picture; + // clear the picture memset(RGB, 0, avframe_rgb_size_); #if LIBAVCODEC_VERSION_MAJOR > 52 @@ -419,12 +459,14 @@ void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels) avpkt.size = len; avpkt.data = (unsigned char*)MJPEG; + AVPixelFormat pix_fmt_backup = avcodec_context_->pix_fmt; + // TODO(lucasw) this corrupts pixel format decoded_len = avcodec_decode_video2(avcodec_context_, avframe_camera_, &got_picture, &avpkt); if (decoded_len < 0) { ROS_ERROR("Error while decoding frame."); - return; + return false; } #else avcodec_decode_video(avcodec_context_, avframe_camera_, &got_picture, (uint8_t *) MJPEG, len); @@ -433,34 +475,46 @@ void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels) if (!got_picture) { ROS_ERROR("Webcam: expected picture but didn't get it..."); - return; + return false; } int xsize = avcodec_context_->width; int ysize = avcodec_context_->height; + // TODO(lucasw) avpicture_get_size corrupts the pix_fmt int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize); + // int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize); if (pic_size != avframe_camera_size_) { ROS_ERROR("outbuf size mismatch. pic_size: %d bufsize: %d", pic_size, avframe_camera_size_); - return; + return false; } - video_sws_ = sws_getContext(xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize, AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, - NULL, NULL); + // TODO(lucasw) why does the image need to be scaled? Does it also convert formats? + // INFO("sw scaler " << xsize << " " << ysize << " " << avcodec_context_->pix_fmt + // << ", linesize " << avframe_rgb_->linesize); + #if 1 + avcodec_context_->pix_fmt = pix_fmt_backup; + // TODO(lucasw) only do if xsize and ysize or pix fmt is different from last time + video_sws_ = sws_getContext(xsize, ysize, avcodec_context_->pix_fmt, + xsize, ysize, AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL); sws_scale(video_sws_, avframe_camera_->data, avframe_camera_->linesize, 0, ysize, avframe_rgb_->data, avframe_rgb_->linesize); + // TODO(lucasw) keep around until parameters change sws_freeContext(video_sws_); + #endif int size = avpicture_layout((AVPicture *)avframe_rgb_, AV_PIX_FMT_RGB24, xsize, ysize, (uint8_t *)RGB, avframe_rgb_size_); if (size != avframe_rgb_size_) { ROS_ERROR("webcam: avpicture_layout error: %d", size); - return; + return false; } + return true; } -void UsbCam::process_image(const void * src, int len, camera_image_t *dest) +bool UsbCam::process_image(const void * src, int len, camera_image_t *dest) { + // TODO(lucasw) return bool from all these if (pixelformat_ == V4L2_PIX_FMT_YUYV) { if (monochrome_) @@ -475,18 +529,23 @@ void UsbCam::process_image(const void * src, int len, camera_image_t *dest) else if (pixelformat_ == V4L2_PIX_FMT_UYVY) uyvy2rgb((char*)src, dest->image, dest->width * dest->height); else if (pixelformat_ == V4L2_PIX_FMT_MJPEG) - mjpeg2rgb((char*)src, len, dest->image, dest->width * dest->height); + return mjpeg2rgb((char*)src, len, dest->image, dest->width * dest->height); else if (pixelformat_ == V4L2_PIX_FMT_RGB24) rgb242rgb((char*)src, dest->image, dest->width * dest->height); else if (pixelformat_ == V4L2_PIX_FMT_GREY) memcpy(dest->image, (char*)src, dest->width * dest->height); + + return true;; } -int UsbCam::read_frame() +bool UsbCam::read_frame() { struct v4l2_buffer buf; unsigned int i; int len; + builtin_interfaces::msg::Time stamp; + timespec buf_time; + timespec real_time; switch (io_) { @@ -497,7 +556,7 @@ int UsbCam::read_frame() switch (errno) { case EAGAIN: - return 0; + return false; case EIO: /* Could ignore EIO, see spec. */ @@ -505,11 +564,14 @@ int UsbCam::read_frame() /* fall through */ default: - errno_exit("read"); + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("read"); } } - process_image(buffers_[0].start, len, image_); + if (!process_image(buffers_[0].start, len, image_)) + return false; + // TODO(lucasw) how to get timestamp with this method? break; @@ -524,7 +586,7 @@ int UsbCam::read_frame() switch (errno) { case EAGAIN: - return 0; + return false; case EIO: /* Could ignore EIO, see spec. */ @@ -532,16 +594,28 @@ int UsbCam::read_frame() /* fall through */ default: - errno_exit("VIDIOC_DQBUF"); + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_DQBUF"); } } + // need to get buf time here otherwise process_image will zero it + TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); + monotonicToRealTime(buf_time, real_time); + stamp.sec = real_time.tv_sec; + stamp.nanosec = real_time.tv_nsec; + assert(buf.index < n_buffers_); len = buf.bytesused; - process_image(buffers_[buf.index].start, len, image_); + if (!process_image(buffers_[buf.index].start, len, image_)) + return false; + + if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) { + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_QBUF"); + } - if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) - errno_exit("VIDIOC_QBUF"); + image_->stamp = stamp; break; @@ -556,7 +630,7 @@ int UsbCam::read_frame() switch (errno) { case EAGAIN: - return 0; + return false; case EIO: /* Could ignore EIO, see spec. */ @@ -564,34 +638,44 @@ int UsbCam::read_frame() /* fall through */ default: - errno_exit("VIDIOC_DQBUF"); + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_DQBUF"); } } + TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); + monotonicToRealTime(buf_time, real_time); + stamp.sec = real_time.tv_sec; + stamp.nanosec = real_time.tv_nsec; + for (i = 0; i < n_buffers_; ++i) if (buf.m.userptr == (unsigned long)buffers_[i].start && buf.length == buffers_[i].length) break; assert(i < n_buffers_); len = buf.bytesused; - process_image((void *)buf.m.userptr, len, image_); + if (!process_image((void *)buf.m.userptr, len, image_)) + return false; - if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) - errno_exit("VIDIOC_QBUF"); + if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) { + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_QBUF"); + } + image_->stamp = stamp; break; } - return 1; + return true; } bool UsbCam::is_capturing() { return is_capturing_; } -void UsbCam::stop_capturing(void) +bool UsbCam::stop_capturing(void) { - if(!is_capturing_) return; + if (!is_capturing_) return false; is_capturing_ = false; enum v4l2_buf_type type; @@ -606,17 +690,20 @@ void UsbCam::stop_capturing(void) case IO_METHOD_USERPTR: type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type)) - errno_exit("VIDIOC_STREAMOFF"); + if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type)) { + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_STREAMOFF"); + } break; } + return true; } -void UsbCam::start_capturing(void) +bool UsbCam::start_capturing(void) { - - if(is_capturing_) return; + if (is_capturing_) + return false; unsigned int i; enum v4l2_buf_type type; @@ -638,15 +725,18 @@ void UsbCam::start_capturing(void) buf.memory = V4L2_MEMORY_MMAP; buf.index = i; - if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) - errno_exit("VIDIOC_QBUF"); + if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) { + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_QBUF"); + } } type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) - errno_exit("VIDIOC_STREAMON"); - + if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) { + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_STREAMON"); + } break; case IO_METHOD_USERPTR: @@ -662,21 +752,26 @@ void UsbCam::start_capturing(void) buf.m.userptr = (unsigned long)buffers_[i].start; buf.length = buffers_[i].length; - if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) - errno_exit("VIDIOC_QBUF"); + if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) { + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_QBUF"); + } } type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) - errno_exit("VIDIOC_STREAMON"); + if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) { + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("VIDIOC_STREAMON"); + } break; } is_capturing_ = true; + return true; } -void UsbCam::uninit_device(void) +bool UsbCam::uninit_device(void) { unsigned int i; @@ -688,8 +783,10 @@ void UsbCam::uninit_device(void) case IO_METHOD_MMAP: for (i = 0; i < n_buffers_; ++i) - if (-1 == munmap(buffers_[i].start, buffers_[i].length)) - errno_exit("munmap"); + if (-1 == munmap(buffers_[i].start, buffers_[i].length)) { + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("munmap"); + } break; case IO_METHOD_USERPTR: @@ -699,16 +796,17 @@ void UsbCam::uninit_device(void) } free(buffers_); + return true; } -void UsbCam::init_read(unsigned int buffer_size) +bool UsbCam::init_read(unsigned int buffer_size) { buffers_ = (buffer*)calloc(1, sizeof(*buffers_)); if (!buffers_) { - ROS_ERROR("Out of memory"); - exit(EXIT_FAILURE); + ERROR("Out of memory"); + return false; } buffers_[0].length = buffer_size; @@ -716,12 +814,13 @@ void UsbCam::init_read(unsigned int buffer_size) if (!buffers_[0].start) { - ROS_ERROR("Out of memory"); - exit(EXIT_FAILURE); + ERROR("Out of memory"); + return false; } + return true; } -void UsbCam::init_mmap(void) +bool UsbCam::init_mmap(void) { struct v4l2_requestbuffers req; @@ -736,18 +835,19 @@ void UsbCam::init_mmap(void) if (EINVAL == errno) { ROS_ERROR_STREAM(camera_dev_ << " does not support memory mapping"); - exit(EXIT_FAILURE); + return false; } else { - errno_exit("VIDIOC_REQBUFS"); + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("VIDIOC_REQBUFS"); } } if (req.count < 2) { ROS_ERROR_STREAM("Insufficient buffer memory on " << camera_dev_); - exit(EXIT_FAILURE); + return false; } buffers_ = (buffer*)calloc(req.count, sizeof(*buffers_)); @@ -755,7 +855,7 @@ void UsbCam::init_mmap(void) if (!buffers_) { ROS_ERROR("Out of memory"); - exit(EXIT_FAILURE); + return false; } for (n_buffers_ = 0; n_buffers_ < req.count; ++n_buffers_) @@ -768,20 +868,25 @@ void UsbCam::init_mmap(void) buf.memory = V4L2_MEMORY_MMAP; buf.index = n_buffers_; - if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf)) - errno_exit("VIDIOC_QUERYBUF"); + if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf)) { + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("VIDIOC_QUERYBUF"); + } buffers_[n_buffers_].length = buf.length; buffers_[n_buffers_].start = mmap(NULL /* start anywhere */, buf.length, PROT_READ | PROT_WRITE /* required */, MAP_SHARED /* recommended */, fd_, buf.m.offset); - if (MAP_FAILED == buffers_[n_buffers_].start) - errno_exit("mmap"); + if (MAP_FAILED == buffers_[n_buffers_].start) { + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("mmap"); + } } + return true; } -void UsbCam::init_userp(unsigned int buffer_size) +bool UsbCam::init_userp(unsigned int buffer_size) { struct v4l2_requestbuffers req; unsigned int page_size; @@ -801,11 +906,12 @@ void UsbCam::init_userp(unsigned int buffer_size) { ROS_ERROR_STREAM(camera_dev_ << " does not support " "user pointer i/o"); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } else { - errno_exit("VIDIOC_REQBUFS"); + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("VIDIOC_REQBUFS"); } } @@ -814,7 +920,7 @@ void UsbCam::init_userp(unsigned int buffer_size) if (!buffers_) { ROS_ERROR("Out of memory"); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } for (n_buffers_ = 0; n_buffers_ < 4; ++n_buffers_) @@ -824,13 +930,14 @@ void UsbCam::init_userp(unsigned int buffer_size) if (!buffers_[n_buffers_].start) { - ROS_ERROR("Out of memory"); - exit(EXIT_FAILURE); + ERROR("Out of memory"); + return false; } } + return true; } -void UsbCam::init_device(int image_width, int image_height, int framerate) +bool UsbCam::init_device(int image_width, int image_height, int framerate) { struct v4l2_capability cap; struct v4l2_cropcap cropcap; @@ -843,18 +950,19 @@ void UsbCam::init_device(int image_width, int image_height, int framerate) if (EINVAL == errno) { ROS_ERROR_STREAM(camera_dev_ << " is no V4L2 device"); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } else { - errno_exit("VIDIOC_QUERYCAP"); + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("VIDIOC_QUERYCAP"); } } if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { ROS_ERROR_STREAM(camera_dev_ << " is no video capture device"); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } switch (io_) @@ -863,7 +971,7 @@ void UsbCam::init_device(int image_width, int image_height, int framerate) if (!(cap.capabilities & V4L2_CAP_READWRITE)) { ROS_ERROR_STREAM(camera_dev_ << " does not support read i/o"); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } break; @@ -873,7 +981,7 @@ void UsbCam::init_device(int image_width, int image_height, int framerate) if (!(cap.capabilities & V4L2_CAP_STREAMING)) { ROS_ERROR_STREAM(camera_dev_ << " does not support streaming i/o"); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } break; @@ -922,8 +1030,10 @@ void UsbCam::init_device(int image_width, int image_height, int framerate) fmt.fmt.pix.pixelformat = pixelformat_; fmt.fmt.pix.field = V4L2_FIELD_INTERLACED; - if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt)) - errno_exit("VIDIOC_S_FMT"); + if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt)) { + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("VIDIOC_S_FMT"); + } /* Note VIDIOC_S_FMT may change width and height. */ @@ -941,17 +1051,18 @@ void UsbCam::init_device(int image_width, int image_height, int framerate) struct v4l2_streamparm stream_params; memset(&stream_params, 0, sizeof(stream_params)); stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0) - errno_exit("Couldn't query v4l fps!"); - - ROS_DEBUG("Capability flag: 0x%x", stream_params.parm.capture.capability); + if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0) { + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("Couldn't query v4l fps!"); + } + INFO("Capability flag: 0x" << std::hex << stream_params.parm.capture.capability << std::dec); stream_params.parm.capture.timeperframe.numerator = 1; stream_params.parm.capture.timeperframe.denominator = framerate; if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0) - ROS_WARN("Couldn't set camera framerate"); + ERROR("Couldn't set camera framerate"); else - ROS_DEBUG("Set framerate to be %i", framerate); + INFO("Set framerate to be " << framerate); switch (io_) { @@ -967,30 +1078,34 @@ void UsbCam::init_device(int image_width, int image_height, int framerate) init_userp(fmt.fmt.pix.sizeimage); break; } + return true; } -void UsbCam::close_device(void) +bool UsbCam::close_device(void) { - if (-1 == close(fd_)) - errno_exit("close"); + if (-1 == close(fd_)) { + std::cerr << "error, quitting, TODO throw " << errno << std::endl; + return false; // ("close"); + } fd_ = -1; + return true; } -void UsbCam::open_device(void) +bool UsbCam::open_device(void) { struct stat st; if (-1 == stat(camera_dev_.c_str(), &st)) { ROS_ERROR_STREAM("Cannot identify '" << camera_dev_ << "': " << errno << ", " << strerror(errno)); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } if (!S_ISCHR(st.st_mode)) { ROS_ERROR_STREAM(camera_dev_ << " is no device"); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } fd_ = open(camera_dev_.c_str(), O_RDWR /* required */| O_NONBLOCK, 0); @@ -998,11 +1113,12 @@ void UsbCam::open_device(void) if (-1 == fd_) { ROS_ERROR_STREAM("Cannot open '" << camera_dev_ << "': " << errno << ", " << strerror(errno)); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } + return true; } -void UsbCam::start(const std::string& dev, io_method io_method, +bool UsbCam::start(const std::string& dev, io_method io_method, pixel_format pixel_format, int image_width, int image_height, int framerate) { @@ -1037,7 +1153,7 @@ void UsbCam::start(const std::string& dev, io_method io_method, else { ROS_ERROR("Unknown pixel format."); - exit(EXIT_FAILURE); + return false; //(EXIT_FAILURE); } open_device(); @@ -1054,9 +1170,10 @@ void UsbCam::start(const std::string& dev, io_method io_method, image_->is_new = 0; image_->image = (char *)calloc(image_->image_size, sizeof(char)); memset(image_->image, 0, image_->image_size * sizeof(char)); + return true; } -void UsbCam::shutdown(void) +bool UsbCam::shutdown(void) { stop_capturing(); uninit_device(); @@ -1077,28 +1194,44 @@ void UsbCam::shutdown(void) if(image_) free(image_); image_ = NULL; + return true; } -void UsbCam::grab_image(sensor_msgs::Image* msg) +bool UsbCam::get_image(builtin_interfaces::msg::Time& stamp, + std::string& encoding, uint32_t& height, uint32_t& width, + uint32_t& step, std::vector& data) { + if ((image_->width == 0) || (image_->height == 0)) + return false; // grab the image - grab_image(); + if (!grab_image()) + return false; + // TODO(lucasw) check if stamp is valid) + // INFO("stamp " << image_->stamp.sec << " " << image_->stamp.nanosec << " to " << stamp.sec << " " << stamp.nanosec); // stamp the image - msg->header.stamp = image_->stamp; - // fill the info + stamp = image_->stamp; + // fill in the info + height = image_->height; + width = image_->width; if (monochrome_) { - fillImage(*msg, "mono8", image_->height, image_->width, image_->width, - image_->image); + encoding = "mono8"; + step = width; } else { - fillImage(*msg, "rgb8", image_->height, image_->width, 3 * image_->width, - image_->image); + // TODO(lucasw) aren't there other encoding types? + encoding = "rgb8"; + step = width * 3; } + // TODO(lucasw) create an Image here and already have the memory allocated, + // eliminate this copy + data.resize(step * height); + memcpy(&data[0], image_->image, data.size()); + return true; } -void UsbCam::grab_image() +bool UsbCam::grab_image() { fd_set fds; struct timeval tv; @@ -1112,28 +1245,33 @@ void UsbCam::grab_image() tv.tv_usec = 0; r = select(fd_ + 1, &fds, NULL, NULL, &tv); - image_->stamp = ros::Time::now(); + // if the v4l2_buffer timestamp isn't available use this time, though + // it may be 10s of milliseconds after the frame acquisition. + image_->stamp = clock_->now(); if (-1 == r) { if (EINTR == errno) - return; + return false; - errno_exit("select"); + std::cerr << "error, quitting " << errno << std::endl; + return false; // ("select"); } if (0 == r) { ROS_ERROR("select timeout"); - exit(EXIT_FAILURE); + return false; } - read_frame(); + if (!read_frame()) + return false; image_->is_new = 1; + return true; } // enables/disables auto focus -void UsbCam::set_auto_focus(int value) +bool UsbCam::set_auto_focus(int value) { struct v4l2_queryctrl queryctrl; struct v4l2_ext_control control; @@ -1145,19 +1283,19 @@ void UsbCam::set_auto_focus(int value) { if (errno != EINVAL) { - perror("VIDIOC_QUERYCTRL"); - return; + ERROR("VIDIOC_QUERYCTRL"); + return false; } else { - ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported"); - return; + ERROR("V4L2_CID_FOCUS_AUTO is not supported"); + return false; } } else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) { - ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported"); - return; + ERROR("V4L2_CID_FOCUS_AUTO is not supported"); + return false; } else { @@ -1167,10 +1305,11 @@ void UsbCam::set_auto_focus(int value) if (-1 == xioctl(fd_, VIDIOC_S_CTRL, &control)) { - perror("VIDIOC_S_CTRL"); - return; + ERROR("VIDIOC_S_CTRL"); + return false; } } + return true; } /** @@ -1179,9 +1318,9 @@ void UsbCam::set_auto_focus(int value) * @param param The name of the parameter to set * @param param The value to assign */ -void UsbCam::set_v4l_parameter(const std::string& param, int value) +bool UsbCam::set_v4l_parameter(const std::string& param, int value) { - set_v4l_parameter(param, boost::lexical_cast(value)); + return set_v4l_parameter(param, boost::lexical_cast(value)); } /** * Set video device parameter via call to v4l-utils. @@ -1189,7 +1328,7 @@ void UsbCam::set_v4l_parameter(const std::string& param, int value) * @param param The name of the parameter to set * @param param The value to assign */ -void UsbCam::set_v4l_parameter(const std::string& param, const std::string& value) +bool UsbCam::set_v4l_parameter(const std::string& param, const std::string& value) { // build the command std::stringstream ss;