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 #include <OgreSceneManager.h>
00033 #include <OgreSceneNode.h>
00034
00035 #ifndef Q_MOC_RUN
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 }
00062
00063 protected Q_SLOTS:
00064 virtual void updateTopic() = 0;
00065
00066 protected:
00067 RosTopicProperty* topic_property_;
00068 };
00069
00076 template<class MessageType>
00077 class MessageFilterDisplay: public _RosTopicDisplay
00078 {
00079
00080 public:
00083 typedef MessageFilterDisplay<MessageType> MFDClass;
00084
00085 MessageFilterDisplay()
00086 : tf_filter_( NULL )
00087 , messages_received_( 0 )
00088 {
00089 QString message_type = QString::fromStdString( ros::message_traits::datatype<MessageType>() );
00090 topic_property_->setMessageType( message_type );
00091 topic_property_->setDescription( message_type + " topic to subscribe to." );
00092 }
00093
00094 virtual void onInitialize()
00095 {
00096 tf_filter_ = new tf::MessageFilter<MessageType>( *context_->getTFClient(),
00097 fixed_frame_.toStdString(), 10, update_nh_ );
00098
00099 tf_filter_->connectInput( sub_ );
00100 tf_filter_->registerCallback( boost::bind( &MessageFilterDisplay<MessageType>::incomingMessage, this, _1 ));
00101 context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00102 }
00103
00104 virtual ~MessageFilterDisplay()
00105 {
00106 unsubscribe();
00107 delete tf_filter_;
00108 }
00109
00110 virtual void reset()
00111 {
00112 Display::reset();
00113 tf_filter_->clear();
00114 messages_received_ = 0;
00115 }
00116
00117 virtual void setTopic( const QString &topic, const QString &datatype )
00118 {
00119 topic_property_->setString( topic );
00120 }
00121
00122 protected:
00123 virtual void updateTopic()
00124 {
00125 unsubscribe();
00126 reset();
00127 subscribe();
00128 context_->queueRender();
00129 }
00130
00131 virtual void subscribe()
00132 {
00133 if( !isEnabled() )
00134 {
00135 return;
00136 }
00137
00138 try
00139 {
00140 sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10 );
00141 setStatus( StatusProperty::Ok, "Topic", "OK" );
00142 }
00143 catch( ros::Exception& e )
00144 {
00145 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00146 }
00147 }
00148
00149 virtual void unsubscribe()
00150 {
00151 sub_.unsubscribe();
00152 }
00153
00154 virtual void onEnable()
00155 {
00156 subscribe();
00157 }
00158
00159 virtual void onDisable()
00160 {
00161 unsubscribe();
00162 reset();
00163 }
00164
00165 virtual void fixedFrameChanged()
00166 {
00167 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00168 reset();
00169 }
00170
00174 void incomingMessage( const typename MessageType::ConstPtr& msg )
00175 {
00176 if( !msg )
00177 {
00178 return;
00179 }
00180
00181 ++messages_received_;
00182 setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00183
00184 processMessage( msg );
00185 }
00186
00190 virtual void processMessage( const typename MessageType::ConstPtr& msg ) = 0;
00191
00192 message_filters::Subscriber<MessageType> sub_;
00193 tf::MessageFilter<MessageType>* tf_filter_;
00194 uint32_t messages_received_;
00195 };
00196
00197 }
00198
00199 #endif // MESSAGE_FILTER_DISPLAY_H