From 26d9dfccf19280d6b0c09d22cc9be9eeb2e73713 Mon Sep 17 00:00:00 2001 From: stsssts Date: Thu, 28 Mar 2019 15:46:51 +0300 Subject: [PATCH 1/4] fix rqt_reconfigure crashes --- libuvc_camera/src/camera_driver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 5b6e611..f483f26 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -51,6 +51,7 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh) ctx_(NULL), dev_(NULL), devh_(NULL), rgb_frame_(NULL), it_(nh_), config_server_(mutex_, priv_nh_), + config_(UVCCameraConfig()), config_changed_(false), cinfo_manager_(nh) { cam_pub_ = it_.advertiseCamera("image_raw", 1, false); From 948ba1006841e86814cebaaaf11f805900b6dc39 Mon Sep 17 00:00:00 2001 From: stsssts Date: Sat, 30 Mar 2019 01:11:15 +0300 Subject: [PATCH 2/4] When the video_mode parameter is compressed the node will publish only compressed image --- .../include/libuvc_camera/camera_driver.h | 1 + libuvc_camera/src/camera_driver.cpp | 109 ++++++++++-------- 2 files changed, 62 insertions(+), 48 deletions(-) diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h index db825fd..efe9068 100644 --- a/libuvc_camera/include/libuvc_camera/camera_driver.h +++ b/libuvc_camera/include/libuvc_camera/camera_driver.h @@ -67,6 +67,7 @@ class CameraDriver { image_transport::ImageTransport it_; image_transport::CameraPublisher cam_pub_; + ros::Publisher compressed_pub_; dynamic_reconfigure::Server config_server_; UVCCameraConfig config_; diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index f483f26..9f53160 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -55,6 +56,7 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh) config_changed_(false), cinfo_manager_(nh) { cam_pub_ = it_.advertiseCamera("image_raw", 1, false); + compressed_pub_ = nh_.advertise("image_raw/compressed", 1); } CameraDriver::~CameraDriver() { @@ -173,62 +175,73 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) { assert(state_ == kRunning); assert(rgb_frame_); - sensor_msgs::Image::Ptr image(new sensor_msgs::Image()); - image->width = config_.width; - image->height = config_.height; - image->step = image->width * 3; - image->data.resize(image->step * image->height); - - if (frame->frame_format == UVC_FRAME_FORMAT_BGR){ - image->encoding = "bgr8"; - memcpy(&(image->data[0]), frame->data, frame->data_bytes); - } else if (frame->frame_format == UVC_FRAME_FORMAT_RGB){ - image->encoding = "rgb8"; - memcpy(&(image->data[0]), frame->data, frame->data_bytes); - } else if (frame->frame_format == UVC_FRAME_FORMAT_UYVY) { - image->encoding = "yuv422"; - memcpy(&(image->data[0]), frame->data, frame->data_bytes); - } else if (frame->frame_format == UVC_FRAME_FORMAT_YUYV) { - // FIXME: uvc_any2bgr does not work on "yuyv" format, so use uvc_yuyv2bgr directly. - uvc_error_t conv_ret = uvc_yuyv2bgr(frame, rgb_frame_); - if (conv_ret != UVC_SUCCESS) { - uvc_perror(conv_ret, "Couldn't convert frame to RGB"); - return; + if(config_.video_mode == "compressed") { + if(frame->frame_format == UVC_FRAME_FORMAT_MJPEG) { + sensor_msgs::CompressedImage::Ptr image(new sensor_msgs::CompressedImage()); + image->data.resize(frame->data_bytes); + image->format = "jpeg"; + + memcpy(&(image->data[0]), frame->data, frame->data_bytes); + compressed_pub_.publish(image); } - image->encoding = "bgr8"; - memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); + } else { + sensor_msgs::Image::Ptr image(new sensor_msgs::Image()); + image->width = config_.width; + image->height = config_.height; + image->step = image->width * 3; + image->data.resize(image->step * image->height); + + if (frame->frame_format == UVC_FRAME_FORMAT_BGR){ + image->encoding = "bgr8"; + memcpy(&(image->data[0]), frame->data, frame->data_bytes); + } else if (frame->frame_format == UVC_FRAME_FORMAT_RGB){ + image->encoding = "rgb8"; + memcpy(&(image->data[0]), frame->data, frame->data_bytes); + } else if (frame->frame_format == UVC_FRAME_FORMAT_UYVY) { + image->encoding = "yuv422"; + memcpy(&(image->data[0]), frame->data, frame->data_bytes); + } else if (frame->frame_format == UVC_FRAME_FORMAT_YUYV) { + // FIXME: uvc_any2bgr does not work on "yuyv" format, so use uvc_yuyv2bgr directly. + uvc_error_t conv_ret = uvc_yuyv2bgr(frame, rgb_frame_); + if (conv_ret != UVC_SUCCESS) { + uvc_perror(conv_ret, "Couldn't convert frame to RGB"); + return; + } + image->encoding = "bgr8"; + memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); #if libuvc_VERSION > 00005 /* version > 0.0.5 */ - } else if (frame->frame_format == UVC_FRAME_FORMAT_MJPEG) { - // Enable mjpeg support despite uvs_any2bgr shortcoming - // https://github.com/ros-drivers/libuvc_ros/commit/7508a09f - uvc_error_t conv_ret = uvc_mjpeg2rgb(frame, rgb_frame_); - if (conv_ret != UVC_SUCCESS) { - uvc_perror(conv_ret, "Couldn't convert frame to RGB"); - return; - } - image->encoding = "rgb8"; - memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); + } else if (frame->frame_format == UVC_FRAME_FORMAT_MJPEG) { + // Enable mjpeg support despite uvs_any2bgr shortcoming + // https://github.com/ros-drivers/libuvc_ros/commit/7508a09f + uvc_error_t conv_ret = uvc_mjpeg2rgb(frame, rgb_frame_); + if (conv_ret != UVC_SUCCESS) { + uvc_perror(conv_ret, "Couldn't convert frame to RGB"); + return; + } + image->encoding = "rgb8"; + memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); #endif - } else { - uvc_error_t conv_ret = uvc_any2bgr(frame, rgb_frame_); - if (conv_ret != UVC_SUCCESS) { - uvc_perror(conv_ret, "Couldn't convert frame to RGB"); - return; + } else { + uvc_error_t conv_ret = uvc_any2bgr(frame, rgb_frame_); + if (conv_ret != UVC_SUCCESS) { + uvc_perror(conv_ret, "Couldn't convert frame to RGB"); + return; + } + image->encoding = "bgr8"; + memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); } - image->encoding = "bgr8"; - memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); - } - sensor_msgs::CameraInfo::Ptr cinfo( - new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo())); + sensor_msgs::CameraInfo::Ptr cinfo( + new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo())); - image->header.frame_id = config_.frame_id; - image->header.stamp = timestamp; - cinfo->header.frame_id = config_.frame_id; - cinfo->header.stamp = timestamp; + image->header.frame_id = config_.frame_id; + image->header.stamp = timestamp; + cinfo->header.frame_id = config_.frame_id; + cinfo->header.stamp = timestamp; - cam_pub_.publish(image, cinfo); + cam_pub_.publish(image, cinfo); + } if (config_changed_) { config_server_.updateConfig(config_); From 8b660b29b203b4cd1a403820c33d675301e0763e Mon Sep 17 00:00:00 2001 From: stsssts Date: Sat, 30 Mar 2019 12:34:06 +0300 Subject: [PATCH 3/4] Adding reconfigure features --- libuvc_camera/src/camera_driver.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 9f53160..162b3ad 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -146,17 +146,17 @@ void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t lev new_config.tilt_absolute = config_.tilt_absolute; } } - // TODO: roll_absolute - // TODO: privacy - // TODO: backlight_compensation - // TODO: contrast - // TODO: power_line_frequency - // TODO: auto_hue - // TODO: saturation - // TODO: sharpness - // TODO: gamma - // TODO: auto_white_balance - // TODO: white_balance_temperature + PARAM_INT(roll_absolute, roll_abs, new_config.roll_absolute); + PARAM_INT(privacy, privacy, new_config.privacy); + PARAM_INT(backlight_compensation, backlight_compensation, new_config.backlight_compensation); + PARAM_INT(contrast, contrast, new_config.contrast); + PARAM_INT(focus_absolute, focus_abs, new_config.focus_absolute); + PARAM_INT(auto_hue, hue_auto, new_config.auto_hue); + PARAM_INT(saturation, saturation, new_config.saturation); + PARAM_INT(sharpness, sharpness, new_config.sharpness); + PARAM_INT(gamma, gamma, new_config.gamma); + PARAM_INT(auto_white_balance, white_balance_temperature_auto, new_config.auto_white_balance); + PARAM_INT(white_balance_temperature, white_balance_temperature_auto, new_config.white_balance_temperature); // TODO: white_balance_BU // TODO: white_balance_RV } From 939a3ef59fa5b555bf73638ce3bcc8743969085c Mon Sep 17 00:00:00 2001 From: stsssts Date: Sun, 31 Mar 2019 23:47:04 +0300 Subject: [PATCH 4/4] Compability with indigo --- libuvc_camera/src/camera_driver.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 162b3ad..4c2bf27 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -136,6 +136,16 @@ void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t lev PARAM_INT(gain, gain, new_config.gain); PARAM_INT(iris_absolute, iris_abs, new_config.iris_absolute); PARAM_INT(brightness, brightness, new_config.brightness); + PARAM_INT(roll_absolute, roll_abs, new_config.roll_absolute); + PARAM_INT(privacy, privacy, new_config.privacy); + PARAM_INT(backlight_compensation, backlight_compensation, new_config.backlight_compensation); + PARAM_INT(contrast, contrast, new_config.contrast); + PARAM_INT(auto_hue, hue_auto, new_config.auto_hue); + PARAM_INT(saturation, saturation, new_config.saturation); + PARAM_INT(sharpness, sharpness, new_config.sharpness); + PARAM_INT(gamma, gamma, new_config.gamma); + PARAM_INT(auto_white_balance, white_balance_temperature_auto, new_config.auto_white_balance); + PARAM_INT(white_balance_temperature, white_balance_temperature_auto, new_config.white_balance_temperature); #endif @@ -146,17 +156,7 @@ void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t lev new_config.tilt_absolute = config_.tilt_absolute; } } - PARAM_INT(roll_absolute, roll_abs, new_config.roll_absolute); - PARAM_INT(privacy, privacy, new_config.privacy); - PARAM_INT(backlight_compensation, backlight_compensation, new_config.backlight_compensation); - PARAM_INT(contrast, contrast, new_config.contrast); PARAM_INT(focus_absolute, focus_abs, new_config.focus_absolute); - PARAM_INT(auto_hue, hue_auto, new_config.auto_hue); - PARAM_INT(saturation, saturation, new_config.saturation); - PARAM_INT(sharpness, sharpness, new_config.sharpness); - PARAM_INT(gamma, gamma, new_config.gamma); - PARAM_INT(auto_white_balance, white_balance_temperature_auto, new_config.auto_white_balance); - PARAM_INT(white_balance_temperature, white_balance_temperature_auto, new_config.white_balance_temperature); // TODO: white_balance_BU // TODO: white_balance_RV }