diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp index 7067291d8..565f9261a 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp @@ -39,15 +39,39 @@ #include // NOLINT: cpplint is unable to handle the include order here #include "get_transport_from_topic.hpp" -#include "image_transport/image_transport.hpp" -#include "image_transport/subscriber_filter.hpp" -#include "rviz_common/ros_topic_display.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include namespace rviz_default_plugins { namespace displays { +enum PluginStatus {SUCCESS, CREATE_FAILURE, LIB_LOAD_FAILURE, DOES_NOT_EXIST}; + +/// \cond +struct TransportDesc +{ + TransportDesc() + : pub_status(DOES_NOT_EXIST), sub_status(DOES_NOT_EXIST) + {} + + std::string package_name; + std::string pub_name; + PluginStatus pub_status; + std::string sub_name; + PluginStatus sub_status; +}; + template class ImageTransportDisplay : public rviz_common::_RosTopicDisplay { @@ -64,6 +88,50 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay QString message_type = QString::fromStdString(rosidl_generator_traits::name()); topic_property_->setMessageType(message_type); topic_property_->setDescription(message_type + " topic to subscribe to."); + + image_transport_type_property_ = new rviz_common::properties::EnumProperty( + "Transport hints", "raw", "Preferred method of sending images.", + this, SLOT(updateTopic())); + + pluginlib::ClassLoader pub_loader( + "image_transport", "image_transport::PublisherPlugin"); + pluginlib::ClassLoader sub_loader( + "image_transport", "image_transport::SubscriberPlugin"); + typedef std::map StatusMap; + StatusMap transports; + + for (const std::string & lookup_name : pub_loader.getDeclaredClasses()) { + std::string transport_name = image_transport::erase_last_copy(lookup_name, "_pub"); + transports[transport_name].pub_name = lookup_name; + transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name); + try { + auto pub = pub_loader.createUniqueInstance(lookup_name); + transports[transport_name].pub_status = SUCCESS; + } catch (const pluginlib::LibraryLoadException &) { + transports[transport_name].pub_status = LIB_LOAD_FAILURE; + } catch (const pluginlib::CreateClassException &) { + transports[transport_name].pub_status = CREATE_FAILURE; + } + } + + for (const std::string & lookup_name : sub_loader.getDeclaredClasses()) { + std::string transport_name = image_transport::erase_last_copy(lookup_name, "_sub"); + transports[transport_name].sub_name = lookup_name; + transports[transport_name].package_name = sub_loader.getClassPackage(lookup_name); + try { + auto sub = sub_loader.createUniqueInstance(lookup_name); + transports[transport_name].sub_status = SUCCESS; + } catch (const pluginlib::LibraryLoadException &) { + transports[transport_name].sub_status = LIB_LOAD_FAILURE; + } catch (const pluginlib::CreateClassException &) { + transports[transport_name].sub_status = CREATE_FAILURE; + } + } + for (const StatusMap::value_type & value : transports) { + std::vector tokens = split( + value.first, '/'); + image_transport_type_property_->addOption(tokens[1].c_str()); + } } /** @@ -92,9 +160,31 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay topic_property_->setString(topic); } + std::vector split(const std::string & target, char c) + { + std::string temp; + std::stringstream stringstream {target}; + std::vector result; + + while (std::getline(stringstream, temp, c)) { + result.push_back(temp); + } + + return result; + } + protected: void updateTopic() override { + if (image_transport_type_property_->getStdString() == "raw") { + QString message_type = + QString::fromStdString(rosidl_generator_traits::name()); + topic_property_->setMessageType(message_type); + } else if (image_transport_type_property_->getStdString() == "compressed") { + QString message_type = + QString::fromStdString(rosidl_generator_traits::name()); + topic_property_->setMessageType(message_type); + } resetSubscription(); } @@ -206,7 +296,6 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay processMessage(msg); } - /// Implement this to process the contents of a message. /** * This is called by incomingMessage(). @@ -218,6 +307,15 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay std::shared_ptr subscription_; rclcpp::Time subscription_start_time_; message_filters::Connection subscription_callback_; + + rviz_common::properties::EnumProperty * image_transport_type_property_; + +private Q_SLOTS: + void updateChoice() + { + std::cerr << "updateChoice" << '\n'; + } + }; } // end namespace displays