topic-multi property working (segfaults on close)

Author: mjforan

rviz_common/CMakeLists.txt

@@ -104,6 +104,7 @@ set(rviz_common_headers_to_moc
   include/rviz_common/properties/property_tree_with_help.hpp
   include/rviz_common/properties/qos_profile_property.hpp
   include/rviz_common/properties/ros_topic_property.hpp
+  include/rviz_common/properties/ros_topic_multi_property.hpp
   include/rviz_common/properties/status_list.hpp
   include/rviz_common/properties/status_property.hpp
   include/rviz_common/properties/string_property.hpp
@@ -183,6 +184,7 @@ set(rviz_common_source_files
   src/rviz_common/properties/property_tree_with_help.cpp
   src/rviz_common/properties/property.cpp
   src/rviz_common/properties/ros_topic_property.cpp
+  src/rviz_common/properties/ros_topic_multi_property.cpp
   src/rviz_common/properties/quaternion_property.cpp
   src/rviz_common/properties/qos_profile_property.cpp
   src/rviz_common/properties/regex_filter_property.cpp

rviz_common/include/rviz_common/properties/ros_topic_multi_property.hpp

@@ -31,7 +31,7 @@
 #define RVIZ_COMMON__PROPERTIES__ROS_TOPIC_MULTI_PROPERTY_HPP_
 
 #include <string>
-#include <unordered_set>
+#include <vector>
 
 #include "rviz_common/properties/ros_topic_property.hpp"
 #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
@@ -46,21 +46,30 @@ class RVIZ_COMMON_PUBLIC RosTopicMultiProperty : public RosTopicProperty
 {
   Q_OBJECT
 public:
+  explicit RosTopicMultiProperty(
+    const QString & name = QString(),
+    const QString & default_value = QString(),
+    const std::vector<QString> & message_types = std::vector<QString>(),
+    const QString & description = QString(),
+    Property * parent = nullptr,
+    const char * changed_slot = nullptr,
+    QObject * receiver = nullptr);
 
-  void setMessageTypes(const std::unordered_set<QString> & message_types);
+  void setMessageTypes(const std::vector<QString> & message_types);
 
-  std::unordered_set<QString> getMessageTypes() const
+  std::vector<QString> getMessageTypes() const
   {return message_types_;}
 
 protected Q_SLOTS:
   virtual void fillTopicList() override;
 
 private:
-  // hide the parent class methods which only take a single type
+  // Hide the parent class methods which only take a single type
   using RosTopicProperty::setMessageType;
   using RosTopicProperty::getMessageType;
 
-  std::unordered_set<QString> message_types_; // TODO is there a QT-friendly type?
+  // Instead of one message type, store a list of allowed types
+  std::vector<QString> message_types_;
 };
 
 }  // end namespace properties

rviz_common/src/rviz_common/properties/ros_topic_multi_property.cpp

@@ -30,6 +30,7 @@
 
 #include <map>
 #include <string>
+#include <algorithm>
 
 #include <QApplication>  // NOLINT: cpplint can't handle Qt imports
 
@@ -41,7 +42,19 @@ namespace rviz_common
 namespace properties
 {
 
-void RosTopicMultiProperty::setMessageTypes(const std::unordered_set<QString> & message_types)
+RosTopicMultiProperty::RosTopicMultiProperty(
+  const QString & name,
+  const QString & default_value,
+  const std::vector<QString> & message_types,
+  const QString & description,
+  Property * parent,
+  const char * changed_slot,
+  QObject * receiver)
+: RosTopicProperty(name, default_value, "", description, parent, changed_slot, receiver),
+  message_types_(message_types)
+{}
+
+void RosTopicMultiProperty::setMessageTypes(const std::vector<QString> & message_types)
 {
   message_types_ = message_types;
 }
@@ -57,9 +70,9 @@ void RosTopicMultiProperty::fillTopicList()
   for (const auto & topic : published_topics) {
     // Only add topics whose type matches.
     for (const auto & type : topic.second) {
-      if (message_types_.find(QString::fromStdString(type)) != message_types_.end()) {
+      if (std::find(message_types_.begin(), message_types_.end(), QString::fromStdString(type))
+      != message_types_.end())
         addOptionStd(topic.first);
-      }
     }
   }
   sortOptions();

rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp

@@ -85,18 +85,13 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public
   ImageDisplay();
   ~ImageDisplay() override;
 
-  // Overrides from Display
   void onInitialize() override;
   void update(float wall_dt, float ros_dt) override;
   void reset() override;
 
-  QString getTransport();
-  std::string getTransportStd();
 
 public Q_SLOTS:
   virtual void updateNormalizeOptions();
-  bool setTransport(const QString & str);
-  bool setTransportStd(const std::string & std_str);
 
 protected:
   // overrides from Display
@@ -111,16 +106,6 @@ public Q_SLOTS:
   void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) override;
 
   image_transport::Subscriber subscription_;
-  rviz_common::properties::EnumProperty* image_transport_property_;
-  const std::unordered_map<QString, QString> transport_message_types_ =
-  {
-      {"raw",        QString::fromStdString(rosidl_generator_traits::name<sensor_msgs::msg::Image>())},
-      {"compressed", QString::fromStdString(rosidl_generator_traits::name<sensor_msgs::msg::CompressedImage>())},
-      /*{"theora",     QString::fromStdString(rosidl_generator_traits::name<theora_image_transport::msg::Packet>())}*/
-  };
-
-protected Q_SLOTS:
-  void updateTransport();
 
 private:
   void setupScreenRectangle();

rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp

@@ -32,6 +32,8 @@
 #include <memory>
 #include <string>
 #include <utility>
+#include <algorithm>
+#include <vector>
 
 #include <OgreCamera.h>
 #include <OgreManualObject.h>
@@ -54,6 +56,7 @@
 #include "rviz_common/render_panel.hpp"
 #include "rviz_common/validate_floats.hpp"
 #include "rviz_common/uniform_string_stream.hpp"
+#include "rviz_common/properties/ros_topic_multi_property.hpp"
 #include "rviz_rendering/material_manager.hpp"
 #include "rviz_rendering/render_window.hpp"
 
@@ -62,6 +65,8 @@
 #include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp"
 #include "rviz_default_plugins/displays/image/get_transport_from_topic.hpp"
 
+#include <rviz_common/logging.hpp> // TODO remove when done
+
 namespace rviz_default_plugins
 {
 namespace displays
@@ -73,16 +78,17 @@ ImageDisplay::ImageDisplay()
 ImageDisplay::ImageDisplay(std::unique_ptr<ROSImageTextureIface> texture)
 : texture_(std::move(texture))
 {
+  delete this->topic_property_;
+  this->topic_property_ = new rviz_common::properties::RosTopicMultiProperty(
+    "Topic",
+    "",
+    std::vector<QString>(),
+    "Image transport topic to subscribe to.",
+    this,
+    SLOT(updateTopic()));
 
-  image_transport_property_ = new rviz_common::properties::EnumProperty(
-    "Image Transport Hint",
-    "raw",
-    "What type of image transport to use.",
-    topic_property_, SLOT(updateTransport()),
-    this);
-
-  for (const auto& [key, value] : transport_message_types_)
-    image_transport_property_->addOption(key);
+  delete this->qos_profile_property_;
+  this->qos_profile_property_ = new rviz_common::properties::QosProfileProperty(this->topic_property_, rclcpp::QoS(5));
 
   normalize_property_ = new rviz_common::properties::BoolProperty(
     "Normalize Range",
@@ -152,6 +158,45 @@ void ImageDisplay::subscribe(){
   if (!isEnabled()) {
     return;
   }
+
+  // TODO this should really be in onInitialize but setStatus does not work there
+  // Populate topic message types based on installed image_transport plugins
+  image_transport::ImageTransport image_transport_(rviz_ros_node_.lock()->get_raw_node());
+  std::vector<std::string> transports = image_transport_.getLoadableTransports();
+  std::vector<QString> message_types;
+  // Map to message types
+  const std::unordered_map<std::string, std::string> transport_message_types_ =
+  {
+      {"raw",             "sensor_msgs/msg/Image"},
+      {"compressed",      "sensor_msgs/msg/CompressedImage"},
+      {"compressedDepth", "sensor_msgs/msg/CompressedImage"},
+      {"theora",          "theora_image_transport/msg/Packet"},
+      {"zstd",            "sensor_msgs/msg/CompressedImage"},
+  };
+  std::string transports_str = "";
+  rviz_common::properties::StatusProperty::Level transports_status_level =
+    rviz_common::properties::StatusProperty::Ok;
+  for (std::string & transport : transports){
+    transport = transport.substr(transport.find_last_of('/') + 1);
+    try{
+      message_types.push_back(QString::fromStdString(transport_message_types_.at(transport)));
+      transports_str += transport + ", ";
+    }
+    catch (const std::out_of_range & e){
+      transports_status_level = rviz_common::properties::StatusProperty::Warn;
+      transports_str += "(unknown: " + transport + "), ";
+    }
+  }
+  setStatusStd(
+      transports_status_level,
+      "Image Transports",
+      transports_str);
+  // Remove duplicates
+  message_types.erase(std::unique(message_types.begin(), message_types.end() ), message_types.end());
+  // Update the message types to allow in the topic_property_
+  ((rviz_common::properties::RosTopicMultiProperty*)topic_property_)->setMessageTypes(message_types);
+
+  RVIZ_COMMON_LOG_INFO("subscribe");
   if (topic_property_->isEmpty()) {
     setStatus(
       rviz_common::properties::StatusProperty::Error, "Topic",
@@ -171,7 +216,10 @@ void ImageDisplay::subscribe(){
       (uint32_t)qos_profile.get_rmw_qos_profile().depth, // TODO try without cast
       &ImageDisplay::incomingMessage,
       this,
-      new image_transport::TransportHints(node.get(),getTransportStd(),"image_transport"));
+      new image_transport::TransportHints(
+        node.get(),
+        getTransportFromTopic(topic_property_->getStdString()),
+        "image_transport"));
 
     setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK");
   } catch (rclcpp::exceptions::InvalidTopicNameError & e) {
@@ -206,14 +254,6 @@ void ImageDisplay::updateNormalizeOptions()
   }
 }
 
-bool ImageDisplay::setTransport(const QString & str){
-  return image_transport_property_->setValue(str.toLower());
-}
-
-bool ImageDisplay::setTransportStd(const std::string & std_str){
-  return image_transport_property_->setValue(QString::fromStdString(std_str).toLower());
-}
-
 void ImageDisplay::clear()
 {
   texture_->clear();
@@ -256,14 +296,6 @@ void ImageDisplay::reset()
   clear();
 }
 
-QString ImageDisplay::getTransport(){
-  return image_transport_property_->getString();
-}
-
-std::string ImageDisplay::getTransportStd(){
-  return image_transport_property_->getStdString();
-}
-
 /* This is called by incomingMessage(). */
 void ImageDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg)
 {
@@ -279,16 +311,6 @@ void ImageDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg)
   texture_->addMessage(msg);
 }
 
-void ImageDisplay::updateTransport(){
-  topic_property_->setMessageType(
-    transport_message_types_.at(image_transport_property_->getString()));
-
-  // If topic does not match desired transport, clear it.
-  if (getTransportFromTopic(topic_property_->getStdString()) !=
-  image_transport_property_->getStdString())
-    topic_property_->setString("");
-}
-
 void ImageDisplay::setupScreenRectangle()
 {
   static int count = 0;