Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <QApplication>
00020
00021 #include <variant_topic_tools/MessageType.h>
00022
00023 #include <rqt_multiplot/MessageEvent.h>
00024
00025 #include "rqt_multiplot/MessageSubscriber.h"
00026
00027 namespace rqt_multiplot {
00028
00029
00030
00031
00032
00033 MessageSubscriber::MessageSubscriber(QObject* parent, const ros::NodeHandle&
00034 nodeHandle) :
00035 QObject(parent),
00036 nodeHandle_(nodeHandle),
00037 queueSize_(100) {
00038 }
00039
00040 MessageSubscriber::~MessageSubscriber() {
00041 }
00042
00043
00044
00045
00046
00047 const ros::NodeHandle& MessageSubscriber::getNodeHandle() const {
00048 return nodeHandle_;
00049 }
00050
00051 const QString& MessageSubscriber::getTopic() const {
00052 return topic_;
00053 }
00054
00055 void MessageSubscriber::setTopic(const QString& topic) {
00056 if (topic != topic_) {
00057 topic_ = topic;
00058
00059 if (subscriber_) {
00060 unsubscribe();
00061 subscribe();
00062 }
00063 }
00064 }
00065
00066 void MessageSubscriber::setQueueSize(size_t queueSize) {
00067 if (queueSize != queueSize_) {
00068 queueSize_ = queueSize;
00069
00070 if (subscriber_) {
00071 unsubscribe();
00072 subscribe();
00073 }
00074 }
00075 }
00076
00077 size_t MessageSubscriber::getQueueSize() const {
00078 return queueSize_;
00079 }
00080
00081 size_t MessageSubscriber::getNumPublishers() const {
00082 return subscriber_.getNumPublishers();
00083 }
00084
00085 bool MessageSubscriber::isValid() const {
00086 return subscriber_;
00087 }
00088
00089
00090
00091
00092
00093 bool MessageSubscriber::event(QEvent* event) {
00094 if (event->type() == MessageEvent::Type) {
00095 MessageEvent* messageEvent = static_cast<MessageEvent*>(event);
00096
00097 emit messageReceived(messageEvent->getTopic(), messageEvent->
00098 getMessage());
00099
00100 return true;
00101 }
00102
00103 return QObject::event(event);
00104 }
00105
00106 void MessageSubscriber::subscribe() {
00107 variant_topic_tools::MessageType type;
00108
00109 subscriber_ = type.subscribe(nodeHandle_, topic_.toStdString(), queueSize_,
00110 boost::bind(&MessageSubscriber::callback, this, _1, _2));
00111
00112 if (subscriber_)
00113 emit subscribed(topic_);
00114 }
00115
00116 void MessageSubscriber::unsubscribe() {
00117 if (subscriber_) {
00118 subscriber_.shutdown();
00119
00120 QApplication::removePostedEvents(this, MessageEvent::Type);
00121
00122 emit unsubscribed(topic_);
00123 }
00124 }
00125
00126 void MessageSubscriber::callback(const variant_topic_tools::MessageVariant&
00127 variant, const ros::Time& receiptTime) {
00128 Message message;
00129
00130 message.setReceiptTime(receiptTime);
00131 message.setVariant(variant);
00132
00133 MessageEvent* messageEvent = new MessageEvent(topic_, message);
00134
00135 QApplication::postEvent(this, messageEvent);
00136 }
00137
00138 #if QT_VERSION >= QT_VERSION_CHECK(5,0,0)
00139 void MessageSubscriber::connectNotify(const QMetaMethod& signal) {
00140 if (signal == QMetaMethod::fromSignal(&MessageSubscriber::messageReceived) &&
00141 !subscriber_)
00142 subscribe();
00143 }
00144
00145 void MessageSubscriber::disconnectNotify(const QMetaMethod& signal) {
00146 if (!receivers(QMetaObject::normalizedSignature(
00147 SIGNAL(messageReceived(const QString&, const Message&))))) {
00148 if (subscriber_)
00149 unsubscribe();
00150
00151 emit aboutToBeDestroyed();
00152
00153 deleteLater();
00154 }
00155 }
00156 #else
00157 void MessageSubscriber::connectNotify(const char* signal) {
00158 if ((QByteArray(signal) == QMetaObject::normalizedSignature(
00159 SIGNAL(messageReceived(const QString&, const Message&)))) &&
00160 !subscriber_)
00161 subscribe();
00162 }
00163
00164 void MessageSubscriber::disconnectNotify(const char* signal) {
00165 if (!receivers(QMetaObject::normalizedSignature(
00166 SIGNAL(messageReceived(const QString&, const Message&))))) {
00167 if (subscriber_)
00168 unsubscribe();
00169
00170 emit aboutToBeDestroyed();
00171
00172 deleteLater();
00173 }
00174 }
00175 #endif
00176
00177 }