Add support for u16; colormaps

Author: jdavidberger

include/web_video_server/image_streamer.h

@@ -74,6 +74,10 @@ class ImageTransportImageStreamer : public ImageStreamer
   bool initialized_;
 
   void imageCallback(const sensor_msgs::ImageConstPtr &msg);
+
+  
+  float min_v_ = NAN, max_v_ = NAN;
+  int colormap_ = -1;
 };
 
 class ImageStreamerType

src/image_streamer.cpp

@@ -24,6 +24,14 @@ ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_
   output_height_ = request.get_query_param_value_or_default<int>("height", -1);
   invert_ = request.has_query_param("invert");
   default_transport_ = request.get_query_param_value_or_default("default_transport", "raw");
+
+  min_v_ = request.get_query_param_value_or_default<float>("min", min_v_);
+  max_v_ = request.get_query_param_value_or_default<float>("max", max_v_);
+  colormap_ = request.get_query_param_value_or_default<int>("colormap", colormap_);
+
+  if(std::isnan(min_v_) && !std::isnan(max_v_)) {
+    min_v_ = 0;
+  }
 }
 
 ImageTransportImageStreamer::~ImageTransportImageStreamer()
@@ -89,19 +97,23 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr
   cv::Mat img;
   try
   {
-    if (msg->encoding.find("F") != std::string::npos)
-    {
-      // scale floating point images
-      cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image;
-      cv::Mat_<float> float_image = float_image_bridge;
-      double max_val;
-      cv::minMaxIdx(float_image, 0, &max_val);
-
-      if (max_val > 0)
-      {
-        float_image *= (255 / max_val);
+    if (msg->encoding.find("16") != std::string::npos || msg->encoding.find("F") != std::string::npos) {
+      cv::Mat image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image;
+
+      double max_val, min_val;
+      if(std::isnan(min_v_) || std::isnan(min_v_)) {
+	cv::minMaxIdx(image_bridge, &min_val, &max_val);
+      } else {
+	min_val = min_v_;
+	max_val = max_v_; 
+      }
+
+      float scale = 255. / (max_val - min_val);
+      image_bridge.convertTo(img, CV_8U, scale, -min_val * scale);
+
+      if(colormap_ >= 0) {
+	cv::applyColorMap(img, img, colormap_);
       }
-      img = float_image;
     }
     else
     {