Change to image_transport to allow compressed images + fix deadlock

include/astra_camera/ob_camera_node.h

@@ -253,7 +253,7 @@ class OBCameraNode {
   std::map<stream_index_pair, std::vector<openni::VideoMode>> supported_video_modes_;
   std::map<stream_index_pair, FrameCallbackFunction> stream_frame_callback_;
   std::map<stream_index_pair, int> unit_step_size_;
-  std::map<stream_index_pair, ros::Publisher> image_publishers_;
+  std::map<stream_index_pair, image_transport::Publisher> image_publishers_;
   std::map<stream_index_pair, ros::Publisher> camera_info_publishers_;
   std::map<stream_index_pair, bool> flip_image_;
 

include/astra_camera/uvc_camera_driver.h

@@ -168,7 +168,7 @@ class UVCCameraDriver {
   ros::ServiceServer set_uvc_mirror_srv_;
   ros::ServiceServer toggle_uvc_camera_srv_;
   ros::ServiceServer save_image_srv_;
-  ros::Publisher image_publisher_;
+  image_transport::Publisher image_publisher_;
   ros::Publisher camera_info_publisher_;
   sensor_msgs::CameraInfo camera_info_;
   std::recursive_mutex device_lock_;

package.xml

@@ -24,4 +24,5 @@
   <depend>tf2</depend>
   <depend>message_filters</depend>
   <depend>camera_info_manager</depend>
+  <depend>libuvc-dev</depend>
 </package>

src/ob_camera_node.cpp

@@ -598,17 +598,18 @@ void OBCameraNode::imageUnsubscribedCallback(const stream_index_pair &stream_ind
 }
 
 void OBCameraNode::setupPublishers() {
+  image_transport::ImageTransport it(nh_);
   for (const auto &stream_index : IMAGE_STREAMS) {
     std::string name = stream_name_[stream_index];
     camera_info_publishers_[stream_index] =
         nh_.advertise<sensor_msgs::CameraInfo>(name + "/camera_info", 1, true);
     if (enable_[stream_index] && device_->hasSensor(stream_index.first)) {
-      ros::SubscriberStatusCallback image_subscribed_cb =
+      image_transport::SubscriberStatusCallback image_subscribed_cb =
           boost::bind(&OBCameraNode::imageSubscribedCallback, this, stream_index);
-      ros::SubscriberStatusCallback image_unsubscribed_cb =
+      image_transport::SubscriberStatusCallback image_unsubscribed_cb =
           boost::bind(&OBCameraNode::imageUnsubscribedCallback, this, stream_index);
-      image_publishers_[stream_index] = nh_.advertise<sensor_msgs::Image>(
-          name + "/image_raw", 1, image_subscribed_cb, image_unsubscribed_cb);
+      image_publishers_[stream_index] =
+          it.advertise(name + "/image_raw", 1, image_subscribed_cb, image_unsubscribed_cb);
     }
   }
 }

src/uvc_camera_driver.cpp

@@ -137,6 +137,7 @@ UVCCameraDriver::UVCCameraDriver(ros::NodeHandle& nh, ros::NodeHandle& nh_privat
     ROS_ERROR_STREAM("init uvc context failed, exit");
     throw std::runtime_error("init uvc context failed");
   }
+  image_transport::ImageTransport it(nh_);
   config_.serial_number = serial_number;
   device_num_ = nh_private_.param<int>("device_num", 1);
   uvc_flip_ = nh_private_.param<bool>("uvc_flip", false);
@@ -163,7 +164,7 @@ UVCCameraDriver::UVCCameraDriver(ros::NodeHandle& nh, ros::NodeHandle& nh_privat
   color_info_manager_ =
       std::make_shared<camera_info_manager::CameraInfoManager>(nh_, "rgb_camera", color_info_uri_);
   std::lock_guard<decltype(device_lock_)> lock(device_lock_);
-  image_publisher_ = nh_.advertise<sensor_msgs::Image>(
+  image_publisher_ =it.advertise(
       "color/image_raw", 10, boost::bind(&UVCCameraDriver::imageSubscribedCallback, this),
       boost::bind(&UVCCameraDriver::imageUnsubscribedCallback, this));
   setupCameraControlService();
@@ -657,7 +658,11 @@ void UVCCameraDriver::frameCallbackWrapper(uvc_frame_t* frame, void* ptr) {
 void UVCCameraDriver::frameCallback(uvc_frame_t* frame) {
   CHECK_NOTNULL(frame);
   CHECK_NOTNULL(frame_buffer_);
-  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  std::unique_lock<decltype(device_lock_)> lock(device_lock_, std::defer_lock);
+  if(!lock.try_lock()) {
+    // if we don't get the lock, just return. Otherwise it might result in a uvc deadlock.
+    return;
+  }
   static constexpr int unit_step = 3;
   sensor_msgs::Image image;
   image.width = frame->width;