diff --git a/CMakeLists.txt b/CMakeLists.txt index b0f3202..85a5285 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/include/rqt_image_view/image_view.h b/include/rqt_image_view/image_view.h index 3734704..948a467 100644 --- a/include/rqt_image_view/image_view.h +++ b/include/rqt_image_view/image_view.h @@ -42,6 +42,7 @@ #include #include #include +#include #include @@ -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_; diff --git a/package.xml b/package.xml index 5acc74d..8800eda 100644 --- a/package.xml +++ b/package.xml @@ -29,7 +29,7 @@ rqt_gui rqt_gui_cpp sensor_msgs - + topic_tools cv_bridge geometry_msgs image_transport diff --git a/src/rqt_image_view/image_view.cpp b/src/rqt_image_view/image_view.cpp index cc8c8c8..9808e24 100644 --- a/src/rqt_image_view/image_view.cpp +++ b/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)