message_field_subscriber.cpp
Go to the documentation of this file.
00001 #include "utilities/message_field.h"
00002 #include "utilities/message_field_subscriber.h"
00003 #include "utilities/message_subscriber_registry.h"
00004 
00005 namespace utilities
00006 {
00007 MessageFieldSubscriber::MessageFieldSubscriber(
00008     QObject* parent, const QString& type, const QString& field,
00009     MessageSubscriberRegistry* registry)
00010     : QObject(parent), subscribed_(false),
00011       field_(new MessageField(this, type, field)), registry_(registry)
00012 {
00013 }
00014 
00015 MessageFieldSubscriber::~MessageFieldSubscriber()
00016 {
00017   registry_ = NULL;
00018   if (field_)
00019   {
00020     delete field_;
00021     field_ = NULL;
00022   }
00023 }
00024 
00025 variant_topic_tools::BuiltinVariant
00026 MessageFieldSubscriber::getCurrentFieldValue() const
00027 {
00028   return current_field_value_;
00029 }
00030 
00031 MessageSubscriberRegistry* MessageFieldSubscriber::getRegistry() const
00032 {
00033   return registry_;
00034 }
00035 
00036 void MessageFieldSubscriber::setRegistry(MessageSubscriberRegistry* registry)
00037 {
00038   if (registry != registry_)
00039   {
00040     QString subscribed_topic(subscribed_topic_);
00041     if (registry_)
00042     {
00043       unsubscribe();
00044     }
00045     registry_ = registry;
00046     if (registry_ && !subscribed_topic.isEmpty())
00047     {
00048       subscribe(subscribed_topic, 10);
00049     }
00050   }
00051 }
00052 
00053 bool MessageFieldSubscriber::isSubscribed() const
00054 {
00055   return !subscribed_topic_.isEmpty();
00056 }
00057 
00058 void MessageFieldSubscriber::subscribe(const QString& topic,
00059                                        const size_t& queue_size)
00060 {
00061   if (isSubscribed())
00062   {
00063     unsubscribe();
00064   }
00065   if (field_ && registry_)
00066   {
00067     MessageBroker::PropertyMap properties;
00068     properties[MessageSubscriber::QueueSize] =
00069         QVariant::fromValue<quint64>(queue_size);
00070     if (registry_->subscribe(topic, this, SLOT(subscriberMessageReceived(
00071                                               const QString&, const Message&)),
00072                              properties))
00073     {
00074       subscribed_topic_ = topic;
00075     }
00076     if (isSubscribed())
00077     {
00078       emit subscribed();
00079     }
00080   }
00081 }
00082 
00083 void MessageFieldSubscriber::unsubscribe()
00084 {
00085   if (isSubscribed())
00086   {
00087     registry_->unsubscribe(subscribed_topic_, this);
00088   }
00089   subscribed_topic_.clear();
00090   emit unsubscribed();
00091 }
00092 
00093 void MessageFieldSubscriber::subscriberMessageReceived(const QString& topic,
00094                                                        const Message& message)
00095 {
00096   try
00097   {
00098     variant_topic_tools::BuiltinVariant field_value(
00099         field_->getFieldValue(message));
00100     emit received(field_value);
00101   }
00102   catch (const ros::Exception& exception)
00103   {
00104     ROS_ERROR_STREAM(
00105         "ROS Exception caught while getting the value of the message field: "
00106         << exception.what());
00107   }
00108 }
00109 }


rqt_mrta
Author(s): Adriano Henrique Rossette Leite
autogenerated on Thu Jun 6 2019 18:50:52