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 #include <message_filters/subscriber.h>
00036 #include <tf/message_filter.h>
00037
00038 #include "rviz/display_context.h"
00039 #include "rviz/frame_manager.h"
00040 #include "rviz/properties/ros_topic_property.h"
00041
00042 #include "rviz/display.h"
00043
00044 namespace rviz
00045 {
00046
00050 class _RosTopicDisplay: public Display
00051 {
00052 Q_OBJECT
00053 public:
00054 _RosTopicDisplay()
00055 {
00056 topic_property_ = new RosTopicProperty( "Topic", "",
00057 "", "",
00058 this, SLOT( updateTopic() ));
00059 }
00060
00061 protected Q_SLOTS:
00062 virtual void updateTopic() = 0;
00063
00064 protected:
00065 RosTopicProperty* topic_property_;
00066 };
00067
00074 template<class MessageType>
00075 class MessageFilterDisplay: public _RosTopicDisplay
00076 {
00077
00078 public:
00081 typedef MessageFilterDisplay<MessageType> MFDClass;
00082
00083 MessageFilterDisplay()
00084 : tf_filter_( NULL )
00085 , scene_node_( NULL )
00086 , messages_received_( 0 )
00087 {
00088 QString message_type = QString::fromStdString( ros::message_traits::datatype<MessageType>() );
00089 topic_property_->setMessageType( message_type );
00090 topic_property_->setDescription( message_type + " topic to subscribe to." );
00091 }
00092
00093 virtual void onInitialize()
00094 {
00095 tf_filter_ = new tf::MessageFilter<MessageType>( *context_->getTFClient(),
00096 fixed_frame_.toStdString(), 10, update_nh_ );
00097
00098 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00099 scene_node_->setVisible( false );
00100
00101 tf_filter_->connectInput( sub_ );
00102 tf_filter_->registerCallback( boost::bind( &MessageFilterDisplay<MessageType>::incomingMessage, this, _1 ));
00103 context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00104 }
00105
00106 virtual ~MessageFilterDisplay()
00107 {
00108 unsubscribe();
00109 delete tf_filter_;
00110 if( scene_node_ )
00111 {
00112 scene_manager_->destroySceneNode( scene_node_ );
00113 }
00114 }
00115
00116 virtual void reset()
00117 {
00118 Display::reset();
00119 tf_filter_->clear();
00120 messages_received_ = 0;
00121 }
00122
00123 protected:
00124 virtual void updateTopic()
00125 {
00126 unsubscribe();
00127 reset();
00128 subscribe();
00129 context_->queueRender();
00130 }
00131
00132 virtual void subscribe()
00133 {
00134 if( !isEnabled() )
00135 {
00136 return;
00137 }
00138
00139 try
00140 {
00141 sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10 );
00142 setStatus( StatusProperty::Ok, "Topic", "OK" );
00143 }
00144 catch( ros::Exception& e )
00145 {
00146 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00147 }
00148 }
00149
00150 virtual void unsubscribe()
00151 {
00152 sub_.unsubscribe();
00153 }
00154
00155 virtual void onEnable()
00156 {
00157 scene_node_->setVisible( true );
00158 subscribe();
00159 }
00160
00161 virtual void onDisable()
00162 {
00163 unsubscribe();
00164 reset();
00165 scene_node_->setVisible( false );
00166 }
00167
00168 virtual void fixedFrameChanged()
00169 {
00170 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00171 reset();
00172 }
00173
00177 void incomingMessage( const typename MessageType::ConstPtr& msg )
00178 {
00179 if( !msg )
00180 {
00181 return;
00182 }
00183
00184 ++messages_received_;
00185 setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00186
00187 processMessage( msg );
00188 }
00189
00193 virtual void processMessage( const typename MessageType::ConstPtr& msg ) = 0;
00194
00195 message_filters::Subscriber<MessageType> sub_;
00196 tf::MessageFilter<MessageType>* tf_filter_;
00197 Ogre::SceneNode* scene_node_;
00198 uint32_t messages_received_;
00199 };
00200
00201 }
00202
00203 #endif // MESSAGE_FILTER_DISPLAY_H