message_filter_display.h

Go to the documentation of this file.

/*
 * Copyright (c) 2012, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#ifndef MESSAGE_FILTER_DISPLAY_H
#define MESSAGE_FILTER_DISPLAY_H
 
#ifndef Q_MOC_RUN
#include <message_filters/subscriber.h>
#include <tf2_ros/message_filter.h>
#endif
 
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/ros_topic_property.h>
 
#include <rviz/display.h>
#include <rviz/rviz_export.h>
 
Q_DECLARE_METATYPE(boost::shared_ptr<const void>)
 
namespace rviz
{
class RVIZ_EXPORT _RosTopicDisplay : public Display
{
  Q_OBJECT
public:
  _RosTopicDisplay()
  {
    topic_property_ = new RosTopicProperty("Topic", "", "", "", this, &_RosTopicDisplay::updateTopic);
    unreliable_property_ = new BoolProperty("Unreliable", false, "Prefer UDP topic transport", this,
                                            &_RosTopicDisplay::updateTopic);
    queue_size_property_ =
        new IntProperty("Queue Size", 10,
                        "Size of TF message filter queue.\n"
                        "Increasing this is useful if your TF data is delayed significantly "
                        "w.r.t. your data, but it can greatly increase memory usage as well.",
                        this, &_RosTopicDisplay::updateQueueSize);
    queue_size_property_->setMin(0);
    qRegisterMetaType<boost::shared_ptr<const void>>(); // required to send via queued connection
  }
 
protected Q_SLOTS:
  virtual void updateTopic() = 0;
  virtual void updateQueueSize() = 0;
private Q_SLOTS:
  virtual void processTypeErasedMessage(boost::shared_ptr<const void> type_erased_msg) = 0;
 
protected:
  RosTopicProperty* topic_property_;
  BoolProperty* unreliable_property_;
  IntProperty* queue_size_property_;
};
 
template <class MessageType>
class MessageFilterDisplay : public _RosTopicDisplay
{
  // No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class.
public:
  typedef MessageFilterDisplay<MessageType> MFDClass;
 
  MessageFilterDisplay() : tf_filter_(nullptr), messages_received_(0)
  {
    QString message_type = QString::fromStdString(ros::message_traits::datatype<MessageType>());
    topic_property_->setMessageType(message_type);
    topic_property_->setDescription(message_type + " topic to subscribe to.");
  }
 
  void onInitialize() override
  {
    tf_filter_ =
        new tf2_ros::MessageFilter<MessageType>(*context_->getTF2BufferPtr(), fixed_frame_.toStdString(),
                                                static_cast<uint32_t>(queue_size_property_->getInt()),
                                                update_nh_);
 
    tf_filter_->connectInput(sub_);
    tf_filter_->registerCallback(
        boost::bind(&MessageFilterDisplay<MessageType>::incomingMessage, this, boost::placeholders::_1));
    context_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
  }
 
  ~MessageFilterDisplay() override
  {
    MessageFilterDisplay::unsubscribe();
    MessageFilterDisplay::reset();
    delete tf_filter_;
  }
 
  void reset() override
  {
    Display::reset();
    tf_filter_->clear();
    messages_received_ = 0;
  }
 
  void setTopic(const QString& topic, const QString& /*datatype*/) override
  {
    topic_property_->setString(topic);
  }
 
protected:
  void updateTopic() override
  {
    unsubscribe();
    reset();
    subscribe();
    context_->queueRender();
  }
 
  void updateQueueSize() override
  {
    tf_filter_->setQueueSize(static_cast<uint32_t>(queue_size_property_->getInt()));
    subscribe();
  }
 
  virtual void subscribe()
  {
    if (!isEnabled())
    {
      return;
    }
 
    try
    {
      ros::TransportHints transport_hint = ros::TransportHints().reliable();
      // Determine UDP vs TCP transport for user selection.
      if (unreliable_property_->getBool())
      {
        transport_hint = ros::TransportHints().unreliable();
      }
      sub_.subscribe(update_nh_, topic_property_->getTopicStd(),
                     static_cast<uint32_t>(queue_size_property_->getInt()), transport_hint);
      setStatus(StatusProperty::Ok, "Topic", "OK");
    }
    catch (ros::Exception& e)
    {
      setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
    }
  }
 
  virtual void unsubscribe()
  {
    sub_.unsubscribe();
  }
 
  void onEnable() override
  {
    subscribe();
  }
 
  void onDisable() override
  {
    unsubscribe();
    reset();
  }
 
  void fixedFrameChanged() override
  {
    tf_filter_->setTargetFrame(fixed_frame_.toStdString());
    reset();
  }
 
  void incomingMessage(const typename MessageType::ConstPtr& msg)
  {
    if (!msg)
    {
      return;
    }
    // process message synchronously in main GUI thread to avoid race conditions
    QMetaObject::invokeMethod(this, "processTypeErasedMessage", Qt::QueuedConnection,
                              Q_ARG(boost::shared_ptr<const void>,
                                    boost::static_pointer_cast<const void>(msg)));
  }
 
  void processTypeErasedMessage(boost::shared_ptr<const void> type_erased_msg) override
  {
    if (!isEnabled())
      return;
 
    auto msg = boost::static_pointer_cast<const MessageType>(type_erased_msg);
    ++messages_received_;
    setStatus(StatusProperty::Ok, "Topic", QString::number(messages_received_) + " messages received");
 
    processMessage(msg);
  }
 
  virtual void processMessage(const typename MessageType::ConstPtr& msg) = 0;
 
  message_filters::Subscriber<MessageType> sub_;
  tf2_ros::MessageFilter<MessageType>* tf_filter_;
  uint32_t messages_received_;
};
 
} // end namespace rviz
 
#endif // MESSAGE_FILTER_DISPLAY_H