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 <OGRE/OgreSceneManager.h>
00033 #include <OGRE/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 protected:
00118 virtual void updateTopic()
00119 {
00120 unsubscribe();
00121 reset();
00122 subscribe();
00123 context_->queueRender();
00124 }
00125
00126 virtual void subscribe()
00127 {
00128 if( !isEnabled() )
00129 {
00130 return;
00131 }
00132
00133 try
00134 {
00135 sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10 );
00136 setStatus( StatusProperty::Ok, "Topic", "OK" );
00137 }
00138 catch( ros::Exception& e )
00139 {
00140 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00141 }
00142 }
00143
00144 virtual void unsubscribe()
00145 {
00146 sub_.unsubscribe();
00147 }
00148
00149 virtual void onEnable()
00150 {
00151 subscribe();
00152 }
00153
00154 virtual void onDisable()
00155 {
00156 unsubscribe();
00157 reset();
00158 }
00159
00160 virtual void fixedFrameChanged()
00161 {
00162 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00163 reset();
00164 }
00165
00169 void incomingMessage( const typename MessageType::ConstPtr& msg )
00170 {
00171 if( !msg )
00172 {
00173 return;
00174 }
00175
00176 ++messages_received_;
00177 setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00178
00179 processMessage( msg );
00180 }
00181
00185 virtual void processMessage( const typename MessageType::ConstPtr& msg ) = 0;
00186
00187 message_filters::Subscriber<MessageType> sub_;
00188 tf::MessageFilter<MessageType>* tf_filter_;
00189 uint32_t messages_received_;
00190 };
00191
00192 }
00193
00194 #endif // MESSAGE_FILTER_DISPLAY_H