Extend the message filter display for point cloud 2 display (#1566)

Author: kbrameld

rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud2_display.hpp

@@ -58,7 +58,6 @@ struct Offsets
   uint32_t x, y, z;
 };
 
-// TODO(greimela) This display originally extended the MessageFilterDisplay. Revisit when available
 /**
  * \class PointCloud2Display
  * \brief Displays a point cloud of type sensor_msgs::PointCloud2

rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_transport_display.hpp

@@ -27,7 +27,6 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-
 #ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_
 #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_
 
@@ -39,186 +38,76 @@
 #include "get_transport_from_topic.hpp"
 #include "point_cloud_transport/point_cloud_transport.hpp"
 #include "point_cloud_transport/subscriber_filter.hpp"
-#include "rviz_common/ros_topic_display.hpp"
+#include "rviz_common/message_filter_display.hpp"
 
 namespace rviz_default_plugins
 {
 namespace displays
 {
 
 template<class MessageType>
-class PointCloud2TransportDisplay : public rviz_common::_RosTopicDisplay
+class PointCloud2TransportDisplay : public rviz_common::MessageFilterDisplay<MessageType>
 {
-// No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class.
+  // No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class.
 
 public:
-/// Convenience typedef so subclasses don't have to use
-/// the long templated class name to refer to their super class.
+  /// Convenience typedef so subclasses don't have to use
+  /// the long templated class name to refer to their super class.
   typedef PointCloud2TransportDisplay<MessageType> PC2RDClass;
 
-  PointCloud2TransportDisplay()
-  : messages_received_(0)
-  {
-    QString message_type = QString::fromStdString(rosidl_generator_traits::name<MessageType>());
-    topic_property_->setMessageType(message_type);
-    topic_property_->setDescription(message_type + " topic to subscribe to.");
-  }
-
-/**
-* When overriding this method, the onInitialize() method of this superclass has to be called.
-* Otherwise, the ros node will not be initialized.
-*/
-  void onInitialize() override
-  {
-    _RosTopicDisplay::onInitialize();
-  }
-
-  ~PointCloud2TransportDisplay() override
-  {
-    unsubscribe();
-  }
-
-  void reset() override
-  {
-    Display::reset();
-    messages_received_ = 0;
-  }
-
-  void setTopic(const QString & topic, const QString & datatype) override
-  {
-    (void) datatype;
-    topic_property_->setString(topic);
-  }
-
 protected:
-  void updateTopic() override
+  void subscribe() override
   {
-    resetSubscription();
-  }
-
-  virtual void subscribe()
-  {
-    if (!isEnabled()) {
+    if (!this->isEnabled()) {
       return;
     }
 
-    if (topic_property_->isEmpty()) {
-      setStatus(
-        rviz_common::properties::StatusProperty::Error, "Topic",
-        QString("Error subscribing: Empty topic name"));
+    if (this->topic_property_->isEmpty()) {
+      this->setStatus(rviz_common::properties::StatusProperty::Error, "Topic",
+                      QString("Error subscribing: Empty topic name"));
       return;
     }
 
     try {
-      subscription_ = std::make_shared<point_cloud_transport::SubscriberFilter>();
-      subscription_->subscribe(
-        *rviz_ros_node_.lock()->get_raw_node(),
-        getPointCloud2BaseTopicFromTopic(topic_property_->getTopicStd()),
-        getPointCloud2TransportFromTopic(topic_property_->getTopicStd()),
-        qos_profile);
-      subscription_start_time_ = rviz_ros_node_.lock()->get_raw_node()->now();
-      subscription_callback_ = subscription_->registerCallback(
-        std::bind(
-          &PointCloud2TransportDisplay<MessageType>::incomingMessage, this, std::placeholders::_1));
-      setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK");
+      rclcpp::Node::SharedPtr node = this->rviz_ros_node_.lock()->get_raw_node();
+      subscriber_filter_ = std::make_shared<point_cloud_transport::SubscriberFilter>();
+      subscriber_filter_->subscribe(
+        rclcpp::node_interfaces::NodeInterfaces<
+          rclcpp::node_interfaces::NodeBaseInterface,
+          rclcpp::node_interfaces::NodeParametersInterface,
+          rclcpp::node_interfaces::NodeTopicsInterface,
+          rclcpp::node_interfaces::NodeLoggingInterface>(*node),
+        getPointCloud2BaseTopicFromTopic(this->topic_property_->getTopicStd()),
+        getPointCloud2TransportFromTopic(this->topic_property_->getTopicStd()),
+        this->qos_profile);
+      this->subscription_start_time_ = node->now();
+      this->tf_filter_ =
+        std::make_shared<tf2_ros::MessageFilter<MessageType,
+          rviz_common::transformation::FrameTransformer>>(
+              *this->context_->getFrameManager()->getTransformer(),
+            this->fixed_frame_.toStdString(),
+              static_cast<uint32_t>(this->message_queue_property_->getInt()), node);
+      this->tf_filter_->connectInput(*subscriber_filter_);
+      this->tf_filter_->registerCallback(
+          std::bind(&PointCloud2TransportDisplay<MessageType>::messageTaken, this,
+            std::placeholders::_1));
+      this->setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK");
     } catch (rclcpp::exceptions::InvalidTopicNameError & e) {
-      setStatus(
-        rviz_common::properties::StatusProperty::Error, "Topic",
-        QString("Error subscribing: ") + e.what());
+      this->setStatus(rviz_common::properties::StatusProperty::Error, "Topic",
+                      QString("Error subscribing: ") + e.what());
     }
   }
 
-  void transformerChangedCallback() override
-  {
-    resetSubscription();
-  }
-
-  void resetSubscription()
-  {
-    unsubscribe();
-    reset();
-    subscribe();
-    context_->queueRender();
-  }
-
   virtual void unsubscribe()
   {
-    subscription_.reset();
-  }
-
-  void onEnable() override
-  {
-    subscribe();
+    rviz_common::MessageFilterDisplay<MessageType>::unsubscribe();
+    subscriber_filter_.reset();
   }
 
-  void onDisable() override
-  {
-    unsubscribe();
-    reset();
-  }
-
-/// Incoming message callback.
-/**
-* Checks if the message pointer
-* is valid, increments messages_received_, then calls
-* processMessage().
-*/
-  void incomingMessage(const typename MessageType::ConstSharedPtr msg)
-  {
-    if (!msg) {
-      return;
-    }
-
-    ++messages_received_;
-    QString topic_str = QString::number(messages_received_) + " messages received";
-    rviz_common::properties::StatusProperty::Level topic_status_level =
-      rviz_common::properties::StatusProperty::Ok;
-    // Append topic subscription frequency if we can lock rviz_ros_node_.
-    std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
-      rviz_ros_node_.lock();
-    if (node_interface != nullptr) {
-      try {
-        const double duration =
-          (node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
-        const double subscription_frequency =
-          static_cast<double>(messages_received_) / duration;
-        topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
-      } catch (const std::runtime_error & e) {
-        if (std::string(e.what()).find("can't subtract times with different time sources") !=
-          std::string::npos)
-        {
-          topic_status_level = rviz_common::properties::StatusProperty::Warn;
-          topic_str += ". ";
-          topic_str += e.what();
-        } else {
-          throw;
-        }
-      }
-    }
-    setStatus(
-      topic_status_level,
-      "Topic",
-      topic_str);
-
-    processMessage(msg);
-  }
-
-
-/// Implement this to process the contents of a message.
-/**
-* This is called by incomingMessage().
-*/
-  virtual void processMessage(typename MessageType::ConstSharedPtr msg) = 0;
-
-  uint32_t messages_received_;
-  rclcpp::Time subscription_start_time_;
-
-  std::shared_ptr<point_cloud_transport::SubscriberFilter> subscription_;
-  message_filters::Connection subscription_callback_;
+  std::shared_ptr<point_cloud_transport::SubscriberFilter> subscriber_filter_;
 };
 
 }  //  end namespace displays
 }  // end namespace rviz_default_plugins
 
-
 #endif  // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_