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
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef MESSAGE_FILTER_DISPLAY_H
00030 #define MESSAGE_FILTER_DISPLAY_H
00031
00032 #ifndef Q_MOC_RUN
00033 #include <OgreSceneManager.h>
00034 #include <OgreSceneNode.h>
00035
00036 #include <message_filters/subscriber.h>
00037 #include <tf/message_filter.h>
00038 #endif
00039
00040 #include "rviz/display_context.h"
00041 #include "rviz/frame_manager.h"
00042 #include "rviz/properties/ros_topic_property.h"
00043
00044 #include "rviz/display.h"
00045
00046 namespace rviz
00047 {
00048
00052 class _RosTopicDisplay: public Display
00053 {
00054 Q_OBJECT
00055 public:
00056 _RosTopicDisplay()
00057 {
00058 topic_property_ = new RosTopicProperty( "Topic", "",
00059 "", "",
00060 this, SLOT( updateTopic() ));
00061 unreliable_property_ = new BoolProperty( "Unreliable", false,
00062 "Prefer UDP topic transport",
00063 this,
00064 SLOT( updateTopic() ));
00065 }
00066
00067 protected Q_SLOTS:
00068 virtual void updateTopic() = 0;
00069
00070 protected:
00071 RosTopicProperty* topic_property_;
00072 BoolProperty* unreliable_property_;
00073 };
00074
00081 template<class MessageType>
00082 class MessageFilterDisplay: public _RosTopicDisplay
00083 {
00084
00085 public:
00088 typedef MessageFilterDisplay<MessageType> MFDClass;
00089
00090 MessageFilterDisplay()
00091 : tf_filter_( NULL )
00092 , messages_received_( 0 )
00093 {
00094 QString message_type = QString::fromStdString( ros::message_traits::datatype<MessageType>() );
00095 topic_property_->setMessageType( message_type );
00096 topic_property_->setDescription( message_type + " topic to subscribe to." );
00097 }
00098
00099 virtual void onInitialize()
00100 {
00101 tf_filter_ = new tf::MessageFilter<MessageType>( *context_->getTFClient(),
00102 fixed_frame_.toStdString(), 10, update_nh_ );
00103
00104 tf_filter_->connectInput( sub_ );
00105 tf_filter_->registerCallback( boost::bind( &MessageFilterDisplay<MessageType>::incomingMessage, this, _1 ));
00106 context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00107 }
00108
00109 virtual ~MessageFilterDisplay()
00110 {
00111 unsubscribe();
00112 delete tf_filter_;
00113 }
00114
00115 virtual void reset()
00116 {
00117 Display::reset();
00118 tf_filter_->clear();
00119 messages_received_ = 0;
00120 }
00121
00122 virtual void setTopic( const QString &topic, const QString &datatype )
00123 {
00124 topic_property_->setString( topic );
00125 }
00126
00127 protected:
00128 virtual void updateTopic()
00129 {
00130 unsubscribe();
00131 reset();
00132 subscribe();
00133 context_->queueRender();
00134 }
00135
00136 virtual void subscribe()
00137 {
00138 if( !isEnabled() )
00139 {
00140 return;
00141 }
00142
00143 try
00144 {
00145 ros::TransportHints transport_hint = ros::TransportHints().reliable();
00146
00147 if (unreliable_property_->getBool())
00148 {
00149 transport_hint = ros::TransportHints().unreliable();
00150 }
00151 sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10, transport_hint);
00152 setStatus( StatusProperty::Ok, "Topic", "OK" );
00153 }
00154 catch( ros::Exception& e )
00155 {
00156 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00157 }
00158 }
00159
00160 virtual void unsubscribe()
00161 {
00162 sub_.unsubscribe();
00163 }
00164
00165 virtual void onEnable()
00166 {
00167 subscribe();
00168 }
00169
00170 virtual void onDisable()
00171 {
00172 unsubscribe();
00173 reset();
00174 }
00175
00176 virtual void fixedFrameChanged()
00177 {
00178 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00179 reset();
00180 }
00181
00185 void incomingMessage( const typename MessageType::ConstPtr& msg )
00186 {
00187 if( !msg )
00188 {
00189 return;
00190 }
00191
00192 ++messages_received_;
00193 setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00194
00195 processMessage( msg );
00196 }
00197
00201 virtual void processMessage( const typename MessageType::ConstPtr& msg ) = 0;
00202
00203 message_filters::Subscriber<MessageType> sub_;
00204 tf::MessageFilter<MessageType>* tf_filter_;
00205 uint32_t messages_received_;
00206 };
00207
00208 }
00209
00210 #endif // MESSAGE_FILTER_DISPLAY_H