Refactors OCS image callbacks, pulling image formatting into a separa…

…te function. Fixes #5
AutoRally · May 24, 2016 · e8dcbfc · elfring · Jun 2, 2016 · barulicm
1 parent 577ee69
commit e8dcbfc
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 32 deletions.
diff --git a/autorally_core/src/ocs/qnode.cpp b/autorally_core/src/ocs/qnode.cpp
@@ -247,21 +247,7 @@ void QNode::imageMaskCallback(const autorally_msgs::imageMask& msg)
 
 void QNode::imageCallback1(const sensor_msgs::ImageConstPtr& msg)
 {
-  QImage::Format format = QImage::Format_Indexed8;
-  unsigned char *newData = 0;
-  if(msg->encoding == "bgr8") {
-    format = QImage::Format_RGB888;
-    newData = convertBGRtoRGB(&msg->data[0], msg->step * msg->height);
-  }
-  if(msg->encoding == "rgb8")
-    format = QImage::Format_RGB888;
-  if(msg->encoding == "mono16")
-    ROS_WARN("The OCS does not currently support 16bpp images. This image topic will not render correctly.");
-  QImage img(msg->encoding == "bgr8" ? newData : &msg->data[0],
-             msg->width,
-             msg->height,
-             msg->step,
-             format);
+  QImage img = formatImageForDisplay(msg);
 
   if(!pthread_mutex_trylock(&m_imageMutex))
   {
@@ -271,26 +257,11 @@ void QNode::imageCallback1(const sensor_msgs::ImageConstPtr& msg)
     }
     pthread_mutex_unlock(&m_imageMutex);
   }
-  delete newData;
 }
 
 void QNode::imageCallback2(const sensor_msgs::ImageConstPtr& msg)
 {
-  QImage::Format format = QImage::Format_Indexed8;
-  unsigned char *newData = 0;
-  if(msg->encoding == "bgr8") {
-    format = QImage::Format_RGB888;
-    newData = convertBGRtoRGB(&msg->data[0], msg->step * msg->height);
-  }
-  if(msg->encoding == "rgb8")
-    format = QImage::Format_RGB888;
-  if(msg->encoding == "mono16")
-    ROS_WARN("The OCS does not currently support 16bpp images. This image topic will not render correctly.");
-  QImage img(msg->encoding == "bgr8" ? newData : &msg->data[0],
-             msg->width,
-             msg->height,
-             msg->step,
-             format);
+  QImage img = formatImageForDisplay(msg);
 
   if(!pthread_mutex_trylock(&m_imageMutex))
   {
@@ -300,7 +271,6 @@ void QNode::imageCallback2(const sensor_msgs::ImageConstPtr& msg)
     }
     pthread_mutex_unlock(&m_imageMutex);
   }
-  delete newData;
 }
 
 unsigned char* QNode::convertBGRtoRGB(const unsigned char* data, int size) {
@@ -401,3 +371,29 @@ void QNode::switchImageTopic(int subscriber, std::string name)
         }
     }
 }
+
+QImage QNode::formatImageForDisplay(const sensor_msgs::ImageConstPtr &msg)
+{
+  QImage::Format format = QImage::Format_Indexed8;
+
+  std::string encoding = msg->encoding;
+
+  unsigned char *newData = nullptr;
+
+  if(encoding == "bgr8") {
+    format = QImage::Format_RGB888;
+    newData = convertBGRtoRGB(&msg->data[0], msg->step * msg->height);
+  }
+  else if(encoding == "rgb8") {
+    format = QImage::Format_RGB888;
+  }
+  else {
+    ROS_WARN_STREAM("The OCS does not currently support " << encoding << " images. This image topic will not render correctly.");
+  }
+
+  QImage img = QImage(encoding == "bgr8" ? newData : &msg->data[0], msg->width, msg->height, msg->step, format).copy();
+
+  delete newData;
+
+  return img;
+}
diff --git a/autorally_core/src/ocs/qnode.hpp b/autorally_core/src/ocs/qnode.hpp
@@ -267,6 +267,9 @@ public slots:
   */
   unsigned char* convertBGRtoRGB(const unsigned char* data, int size);
 
+
+  QImage formatImageForDisplay(const sensor_msgs::ImageConstPtr& msg);
+
 };
 
 #endif /* QNODE_HPP_ */