Fall back to mjpeg if ros_compressed is unavailable (RobotWebTools#87)

Author: sfalexrog

src/web_video_server.cpp

@@ -131,6 +131,28 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ
   std::string type = request.get_query_param_value_or_default("type", __default_stream_type);
   if (stream_types_.find(type) != stream_types_.end())
   {
+    std::string topic = request.get_query_param_value_or_default("topic", "");
+    // Fallback for topics without corresponding compressed topics
+    if (type == std::string("ros_compressed"))
+    {
+      std::string compressed_topic_name = topic + "/compressed";
+      ros::master::V_TopicInfo topics;
+      ros::master::getTopics(topics);
+      bool did_find_compressed_topic = false;
+      for(ros::master::V_TopicInfo::iterator it = topics.begin(); it != topics.end(); ++it)
+      {
+        if (it->name == compressed_topic_name)
+        {
+          did_find_compressed_topic = true;
+          break;
+        }
+      }
+      if (!did_find_compressed_topic)
+      {
+        ROS_WARN_STREAM("Could not find compressed image topic for " << topic << ", falling back to mjpeg");
+        type = "mjpeg";
+      }
+    }
     boost::shared_ptr<ImageStreamer> streamer = stream_types_[type]->create_streamer(request, connection, nh_);
     streamer->start();
     boost::mutex::scoped_lock lock(subscriber_mutex_);
@@ -160,10 +182,32 @@ bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpReques
                                           async_web_server_cpp::HttpConnectionPtr connection, const char* begin,
                                           const char* end)
 {
-  std::string type = request.get_query_param_value_or_default("type", "mjpeg");
+  std::string type = request.get_query_param_value_or_default("type", __default_stream_type);
   if (stream_types_.find(type) != stream_types_.end())
   {
     std::string topic = request.get_query_param_value_or_default("topic", "");
+    // Fallback for topics without corresponding compressed topics
+    if (type == std::string("ros_compressed"))
+    {
+      
+      std::string compressed_topic_name = topic + "/compressed";
+      ros::master::V_TopicInfo topics;
+      ros::master::getTopics(topics);
+      bool did_find_compressed_topic = false;
+      for(ros::master::V_TopicInfo::iterator it = topics.begin(); it != topics.end(); ++it)
+      {
+        if (it->name == compressed_topic_name)
+        {
+          did_find_compressed_topic = true;
+          break;
+        }
+      }
+      if (!did_find_compressed_topic)
+      {
+        ROS_WARN_STREAM("Could not find compressed image topic for " << topic << ", falling back to mjpeg");
+        type = "mjpeg";
+      }
+    }
 
     async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header(
         "Server", "web_video_server").header("Content-type", "text/html;").write(connection);