Lazy detect transport

CMakeLists.txt

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.0.2)
 project(rqt_image_view)
 
 # Load catkin and all dependencies required for this package
-find_package(catkin REQUIRED COMPONENTS rqt_gui rqt_gui_cpp image_transport sensor_msgs geometry_msgs cv_bridge)
+find_package(catkin REQUIRED COMPONENTS rqt_gui rqt_gui_cpp image_transport sensor_msgs geometry_msgs cv_bridge topic_tools)
 
 if("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ")
   find_package(Qt5Widgets REQUIRED)
@@ -37,7 +37,7 @@ endif()
 catkin_package(
   INCLUDE_DIRS ${rqt_image_view_INCLUDE_DIRECTORIES}
   LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS rqt_gui rqt_gui_cpp image_transport sensor_msgs cv_bridge geometry_msgs
+  CATKIN_DEPENDS rqt_gui rqt_gui_cpp image_transport sensor_msgs cv_bridge geometry_msgs topic_tools
 )
 catkin_python_setup()
 

include/rqt_image_view/image_view.h

@@ -42,6 +42,7 @@
 #include <ros/macros.h>
 #include <sensor_msgs/Image.h>
 #include <geometry_msgs/Point.h>
+#include <topic_tools/shape_shifter.h>
 
 #include <opencv2/core/core.hpp>
 
@@ -116,6 +117,7 @@ protected slots:
 protected:
 
   virtual void callbackImage(const sensor_msgs::Image::ConstPtr& msg);
+  virtual void callbackMessage(const topic_tools::ShapeShifter::ConstPtr &msg);
 
   virtual void invertPixels(int x, int y);
 
@@ -128,6 +130,7 @@ protected slots:
   QWidget* widget_;
 
   image_transport::Subscriber subscriber_;
+  ros::Subscriber subscriber_shape_shifter_;
 
   cv::Mat conversion_mat_;
 

package.xml

@@ -29,7 +29,7 @@
   <build_depend>rqt_gui</build_depend>
   <build_depend>rqt_gui_cpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
-
+  <depend>topic_tools</depend>
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>image_transport</exec_depend>

src/rqt_image_view/image_view.cpp

@@ -334,17 +334,26 @@ void ImageView::onTopicChanged(int index)
 
   QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" ");
   QString topic = parts.first();
-  QString transport = parts.length() == 2 ? parts.last() : "raw";
-
   if (!topic.isEmpty())
   {
-    image_transport::ImageTransport it(getNodeHandle());
-    image_transport::TransportHints hints(transport.toStdString());
-    try {
-      subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, hints);
-      //qDebug("ImageView::onTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), subscriber_.getTransport().c_str());
-    } catch (image_transport::TransportLoadException& e) {
-      QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what());
+    if (parts.length() != 2)
+    {
+      // we cannot identify transport here, determine it after receiving the first message.
+      subscriber_shape_shifter_ =
+        getNodeHandle().subscribe(topic.toStdString(), 1, &ImageView::callbackMessage, this);
+    }
+    else
+    {
+      QString transport = parts.last();
+
+      image_transport::ImageTransport it(getNodeHandle());
+      image_transport::TransportHints hints(transport.toStdString());
+      try {
+        subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, hints);
+      } catch (image_transport::TransportLoadException& e)
+      {
+        QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what());
+      }
     }
   }
 
@@ -666,6 +675,29 @@ void ImageView::callbackImage(const sensor_msgs::Image::ConstPtr& msg)
   // though could check and see if the aspect ratio changed or not.
   onZoom1(ui_.zoom_1_push_button->isChecked());
 }
+
+void ImageView::callbackMessage(const topic_tools::ShapeShifter::ConstPtr& msg)
+{
+  image_transport::ImageTransport it(getNodeHandle());
+  std::string topic = subscriber_shape_shifter_.getTopic();
+  std::string transport = "raw";
+  if (msg->getDataType() == "sensor_msgs/CompressedImage")
+  {
+    QString q_topic(topic.c_str());
+    topic = q_topic.mid(0, q_topic.lastIndexOf("/")).toStdString();
+    transport = q_topic.mid(q_topic.lastIndexOf("/") + 1).toStdString();
+  }
+  image_transport::TransportHints hints(transport);
+  try
+  {
+    subscriber_ = it.subscribe(topic, 1, &ImageView::callbackImage, this, hints);
+    subscriber_shape_shifter_.shutdown();
+  }
+  catch (const image_transport::TransportLoadException& e)
+  {
+    ROS_ERROR_STREAM("Loading image transport plugin failed: " << e.what());
+  }
+}
 }
 
 PLUGINLIB_EXPORT_CLASS(rqt_image_view::ImageView, rqt_gui_cpp::Plugin)