Add support for u16; colormaps

include/web_video_server/image_streamer.h

@@ -65,7 +65,7 @@ class ImageTransportImageStreamer : public ImageStreamer
   bool invert_;
   std::string default_transport_;
 
-  ros::Time last_frame;
+  ros::WallTime last_frame;
   cv::Mat output_size_image;
   boost::mutex send_mutex_;
 
@@ -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

include/web_video_server/multipart_stream.h

@@ -10,7 +10,7 @@ namespace web_video_server
 {
 
 struct PendingFooter {
-  ros::Time timestamp;
+  ros::WallTime timestamp;
   boost::weak_ptr<std::string> contents;
 };
 

include/web_video_server/web_video_server.h

@@ -49,6 +49,9 @@ class WebVideoServer
   bool handle_list_streams(const async_web_server_cpp::HttpRequest &request,
                            async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end);
 
+  bool handle_list_streams_json(const async_web_server_cpp::HttpRequest &request,
+                           async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end);
+
 private:
   void restreamFrames(double max_age);
   void cleanup_inactive_streams();

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()
@@ -55,7 +63,7 @@ void ImageTransportImageStreamer::restreamFrame(double max_age)
   if (inactive_ || !initialized_ )
     return;
   try {
-    if ( last_frame + ros::Duration(max_age) < ros::Time::now() ) {
+    if ( last_frame + ros::WallDuration(max_age) < ros::WallTime::now() ) {
       boost::mutex::scoped_lock lock(send_mutex_);
       sendImage(output_size_image, ros::Time::now() ); // don't update last_frame, it may remain an old value.
     }
@@ -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
     {
@@ -142,7 +154,7 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr
       initialized_ = true;
     }
 
-    last_frame = ros::Time::now();
+    last_frame = ros::WallTime::now();
     sendImage(output_size_image, msg->header.stamp);
   }
   catch (cv_bridge::Exception &e)

src/multipart_stream.cpp

@@ -15,9 +15,10 @@ void MultipartStream::sendInitialHeader() {
   async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header(
       "Server", "web_video_server").header("Cache-Control",
                                            "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").header(
-      "Pragma", "no-cache").header("Content-type", "multipart/x-mixed-replace;boundary="+boundry_).header(
-      "Access-Control-Allow-Origin", "*").header("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,HEAD,OPTIONS").header(
-      "Access-Control-Allow-Headers", "Origin, Authorization, Accept, Content-Type").header("Access-Control-Max-Age", "3600").write(connection_);
+      "Pragma", "no-cache").header("Content-type", "multipart/x-mixed-replace;boundary="+boundry_)
+    .header("Access-Control-Allow-Origin", "*").header("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,HEAD,OPTIONS")
+    .header("Access-Control-Allow-Headers", "Origin, Authorization, Accept, Content-Type")
+    .header("Access-Control-Max-Age", "3600").write(connection_);
   connection_->write("--"+boundry_+"\r\n");
 }
 
@@ -40,7 +41,7 @@ void MultipartStream::sendPartHeader(const ros::Time &time, const std::string& t
 void MultipartStream::sendPartFooter(const ros::Time &time) {
   boost::shared_ptr<std::string> str(new std::string("\r\n--"+boundry_+"\r\n"));
   PendingFooter pf;
-  pf.timestamp = time;
+  pf.timestamp = ros::WallTime::now();
   pf.contents = str;
   connection_->write(boost::asio::buffer(*str), str);
   if (max_queue_size_ > 0) pending_footers_.push(pf);
@@ -68,13 +69,13 @@ void MultipartStream::sendPart(const ros::Time &time, const std::string& type,
 }
 
 bool MultipartStream::isBusy() {
-  ros::Time currentTime = ros::Time::now();
+  auto currentTime = ros::WallTime::now();
   while (!pending_footers_.empty())
   {
     if (pending_footers_.front().contents.expired()) {
       pending_footers_.pop();
     } else {
-      ros::Time footerTime = pending_footers_.front().timestamp;
+      auto footerTime = pending_footers_.front().timestamp;
       if ((currentTime - footerTime).toSec() > 0.5) {
         pending_footers_.pop();
       } else {

src/web_video_server.cpp

@@ -33,6 +33,11 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler
   try
   {
     forward(request, connection, begin, end);
+    if (__verbose)
+    {
+      ROS_INFO_STREAM("Request forwarded: " << request.uri);
+    }
+
     return true;
   }
   catch (std::exception &e)
@@ -64,6 +69,9 @@ WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
   private_nh.param("ros_threads", ros_threads_, 2);
   private_nh.param("publish_rate", publish_rate_, -1.0);
 
+  if(__verbose) {
+      ROS_INFO_STREAM("Starting with args server threads: " << server_threads << " ros_threads: " << ros_threads_);
+  }
   private_nh.param<std::string>("default_stream_type", __default_stream_type, "mjpeg");
 
   stream_types_["mjpeg"] = boost::shared_ptr<ImageStreamerType>(new MjpegStreamerType());
@@ -74,6 +82,7 @@ WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
   stream_types_["vp9"] = boost::shared_ptr<ImageStreamerType>(new Vp9StreamerType());
 
   handler_group_.addHandlerForPath("/", boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2, _3, _4));
+  handler_group_.addHandlerForPath("/json", boost::bind(&WebVideoServer::handle_list_streams_json, this, _1, _2, _3, _4));
   handler_group_.addHandlerForPath("/stream", boost::bind(&WebVideoServer::handle_stream, this, _1, _2, _3, _4));
   handler_group_.addHandlerForPath("/stream_viewer",
                                    boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2, _3, _4));
@@ -100,7 +109,7 @@ WebVideoServer::~WebVideoServer()
 void WebVideoServer::spin()
 {
   server_->run();
-  ROS_INFO_STREAM("Waiting For connections on " << address_ << ":" << port_);
+    ROS_INFO_STREAM("Waiting For connections on " << address_ << ":" << port_);
 
   ros::AsyncSpinner spinner(ros_threads_);
   spinner.start();
@@ -183,6 +192,10 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ
     streamer->start();
     boost::mutex::scoped_lock lock(subscriber_mutex_);
     image_subscribers_.push_back(streamer);
+      if (__verbose)
+      {
+          ROS_INFO_STREAM("Serving " << image_subscribers_.size() << " streams");
+      }
   }
   else
   {
@@ -342,8 +355,72 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest
   return true;
 }
 
-}
+bool WebVideoServer::handle_list_streams_json(const async_web_server_cpp::HttpRequest &request,
+                                         async_web_server_cpp::HttpConnectionPtr connection, const char* begin,
+                                         const char* end)
+{
+  std::string image_message_type = ros::message_traits::datatype<sensor_msgs::Image>();
+  std::string camera_info_message_type = ros::message_traits::datatype<sensor_msgs::CameraInfo>();
 
+  ros::master::V_TopicInfo topics;
+  ros::master::getTopics(topics);
+  ros::master::V_TopicInfo::iterator it;
+  std::vector<std::string> image_topics;
+  std::vector<std::string> camera_info_topics;
+  for (it = topics.begin(); it != topics.end(); ++it)
+  {
+    const ros::master::TopicInfo &topic = *it;
+    if (topic.datatype == image_message_type)
+    {
+      image_topics.push_back(topic.name);
+    }
+    else if (topic.datatype == camera_info_message_type)
+    {
+      camera_info_topics.push_back(topic.name);
+    }
+  }
+
+  async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header(
+      "Server", "web_video_server").header("Cache-Control", 
+                                           "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").
+    header("Pragma", "no-cache").
+    header("Content-type", "application/json;").
+    header("Access-Control-Allow-Headers", "*").
+    header("Keep-Alive", "timeout=1").
+    header("Access-Control-Allow-Origin", "*").write(connection);
+
+  connection->write("[");
+  bool first = true;
+  BOOST_FOREACH(std::string & camera_info_topic, camera_info_topics)    
+  {
+    if (boost::algorithm::ends_with(camera_info_topic, "/camera_info"))
+    {
+      std::string base_topic = camera_info_topic.substr(0, camera_info_topic.size() - strlen("camera_info"));
+      std::vector<std::string>::iterator image_topic_itr = image_topics.begin();
+      for (; image_topic_itr != image_topics.end();)
+      {
+        if (boost::starts_with(*image_topic_itr, base_topic))
+        {
+	  if(!first)
+	    connection->write(",");
+	  first = false;
+          connection->write("\"");
+          connection->write(*image_topic_itr);
+          connection->write("\"");
+
+          image_topic_itr = image_topics.erase(image_topic_itr);
+        }
+        else
+        {
+          ++image_topic_itr;
+        }
+      }
+    }
+  }
+  connection->write("]");
+  return true;
+}
+}
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "web_video_server");