ambient_sound_display.cpp
Go to the documentation of this file.
00001 #include <OGRE/OgreSceneNode.h>
00002 #include <OGRE/OgreSceneManager.h>
00003 
00004 #include <tf/transform_listener.h>
00005 
00006 #include <rviz/visualization_manager.h>
00007 #include <rviz/properties/property.h>
00008 #include <rviz/properties/property_manager.h>
00009 #include <rviz/frame_manager.h>
00010 
00011 #include "ambient_sound_visual.h"
00012 
00013 #include "ambient_sound_display.h"
00014 
00015 namespace jsk_rviz_plugin
00016 {
00017 
00018     // BEGIN_TUTORIAL
00019     // The constructor must have no arguments, so we can't give the
00020     // constructor the parameters it needs to fully initialize.
00021     AmbientSoundDisplay::AmbientSoundDisplay()/*{{{*/
00022         : Display()
00023           , scene_node_( NULL )
00024           , messages_received_( 0 )
00025           , color_( .8, .2, .8 )       // Default color is bright purple.
00026           , alpha_( 1.0 )              // Default alpha is completely opaque.
00027           , width_(0.1)
00028           , scale_(0.1)
00029     {
00030     }/*}}}*/
00031 
00032     // After the parent rviz::Display::initialize() does its own setup, it
00033     // calls the subclass's onInitialize() function.  This is where we
00034     // instantiate all the workings of the class.
00035     void AmbientSoundDisplay::onInitialize()/*{{{*/
00036     {
00037         // Make an Ogre::SceneNode to contain all our visuals.
00038         scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00039 
00040         // Set the default history length and resize the ``visuals_`` array.
00041         setHistoryLength( 1 );
00042 
00043         // A tf::MessageFilter listens to ROS messages and calls our
00044         // callback with them when they can be matched up with valid tf
00045         // transform data.
00046         tf_filter_ =
00047             new tf::MessageFilter<hark_msgs::HarkPower>( *vis_manager_->getTFClient(),
00048                     "", 100, update_nh_ );
00049         tf_filter_->connectInput( sub_ );
00050         tf_filter_->registerCallback( boost::bind( &AmbientSoundDisplay::incomingMessage,
00051                     this, _1 ));
00052 
00053         // FrameManager has some built-in functions to set the status of a
00054         // Display based on callbacks from a tf::MessageFilter.  These work
00055         // fine for this simple display.
00056         vis_manager_->getFrameManager()
00057             ->registerFilterForTransformStatusCheck( tf_filter_, this );
00058     }/*}}}*/
00059 
00060     AmbientSoundDisplay::~AmbientSoundDisplay()/*{{{*/
00061     {
00062         unsubscribe();
00063         clear();
00064         for( size_t i = 0; i < visuals_.size(); i++ )
00065         {
00066             delete visuals_[ i ];
00067         }
00068 
00069         delete tf_filter_;
00070     }/*}}}*/
00071 
00072     // Clear the visuals by deleting their objects.
00073     void AmbientSoundDisplay::clear()/*{{{*/
00074     {
00075         for( size_t i = 0; i < visuals_.size(); i++ )
00076         {
00077             delete visuals_[ i ];
00078             visuals_[ i ] = NULL;
00079         }
00080         tf_filter_->clear();
00081         messages_received_ = 0;
00082         setStatus( rviz::status_levels::Warn, "Topic", "No messages received" );
00083     }/*}}}*/
00084 
00085     void AmbientSoundDisplay::setTopic( const std::string& topic )/*{{{*/
00086     {
00087         unsubscribe();
00088         clear();
00089         topic_ = topic;
00090         subscribe();
00091 
00092         // Broadcast the fact that the variable has changed.
00093         propertyChanged( topic_property_ );
00094 
00095         // Make sure rviz renders the next time it gets a chance.
00096         causeRender();
00097     }/*}}}*/
00098 
00099     void AmbientSoundDisplay::setColor( const rviz::Color& color )/*{{{*/
00100     {
00101         color_ = color;
00102 
00103         propertyChanged( color_property_ );
00104         updateColorAndAlpha();
00105         causeRender();
00106     }/*}}}*/
00107 
00108     void AmbientSoundDisplay::setAlpha( float alpha )/*{{{*/
00109     {
00110         alpha_ = alpha;
00111 
00112         propertyChanged( alpha_property_ );
00113         updateColorAndAlpha();
00114         causeRender();
00115     }/*}}}*/
00116 
00117     void AmbientSoundDisplay::setWidth( float width )/*{{{*/
00118     {
00119         width_ = width;
00120         
00121         propertyChanged( width_property_ );
00122         updateColorAndAlpha();
00123         causeRender();
00124     }/*}}}*/
00125 
00126     void AmbientSoundDisplay::setScale( float scale )/*{{{*/
00127     {
00128         scale_ = scale;
00129         
00130         propertyChanged( scale_property_ );
00131         updateColorAndAlpha();
00132         causeRender();
00133     }
00134     /*}}}*/
00135     
00136     void AmbientSoundDisplay::setBias( float bias )/*{{{*/
00137     {
00138         bias_ = bias;
00139         
00140         propertyChanged( bias_property_ );
00141         updateColorAndAlpha();
00142         causeRender();
00143     }/*}}}*/
00144 
00145     void AmbientSoundDisplay::setGrad( float grad )/*{{{*/
00146     {
00147         grad_ = grad;
00148         
00149         propertyChanged( grad_property_ );
00150         updateColorAndAlpha();
00151         causeRender();
00152     }/*}}}*/
00153 
00154     // Set the current color and alpha values for each visual.
00155     void AmbientSoundDisplay::updateColorAndAlpha()/*{{{*/
00156     {
00157         for( size_t i = 0; i < visuals_.size(); i++ )
00158         {
00159             if( visuals_[ i ] )
00160             {
00161                 visuals_[ i ]->setColor( color_.r_, color_.g_, color_.b_, alpha_ );
00162             }
00163         }
00164     }/*}}}*/
00165 
00166     // Set the number of past visuals to show.
00167     void AmbientSoundDisplay::setHistoryLength( int length )/*{{{*/
00168     {
00169         // Don't let people enter invalid values.
00170         if( length < 1 )
00171         {
00172             length = 1;
00173         }
00174         // If the length is not changing, we don't need to do anything.
00175         if( history_length_ == length )
00176         {
00177             return;
00178         }
00179 
00180         // Set the actual variable.
00181         history_length_ = length;
00182         propertyChanged( history_length_property_ );
00183 
00184         // Create a new array of visual pointers, all NULL.
00185         std::vector<AmbientSoundVisual*> new_visuals( history_length_, (AmbientSoundVisual*)0 );
00186 
00187         // Copy the contents from the old array to the new.
00188         // (Number to copy is the minimum of the 2 vector lengths).
00189         size_t copy_len =
00190             (new_visuals.size() > visuals_.size()) ?
00191             visuals_.size() : new_visuals.size();
00192         for( size_t i = 0; i < copy_len; i++ )
00193         {
00194             int new_index = (messages_received_ - i) % new_visuals.size();
00195             int old_index = (messages_received_ - i) % visuals_.size();
00196             new_visuals[ new_index ] = visuals_[ old_index ];
00197             visuals_[ old_index ] = NULL;
00198         }
00199 
00200         // Delete any remaining old visuals
00201         for( size_t i = 0; i < visuals_.size(); i++ ) {
00202             delete visuals_[ i ];
00203         }
00204 
00205         // We don't need to create any new visuals here, they are created as
00206         // needed when messages are received.
00207 
00208         // Put the new vector into the member variable version and let the
00209         // old one go out of scope.
00210         visuals_.swap( new_visuals );
00211     }/*}}}*/
00212 
00213     void AmbientSoundDisplay::subscribe()/*{{{*/
00214     {
00215         // If we are not actually enabled, don't do it.
00216         if ( !isEnabled() )
00217         {
00218             return;
00219         }
00220 
00221         // Try to subscribe to the current topic name (in ``topic_``).  Make
00222         // sure to catch exceptions and set the status to a descriptive
00223         // error message.
00224         try
00225         {
00226             sub_.subscribe( update_nh_, topic_, 10 );
00227             setStatus( rviz::status_levels::Ok, "Topic", "OK" );
00228         }
00229         catch( ros::Exception& e )
00230         {
00231             setStatus( rviz::status_levels::Error, "Topic",
00232                     std::string( "Error subscribing: " ) + e.what() );
00233         }
00234     }/*}}}*/
00235 
00236     void AmbientSoundDisplay::unsubscribe()/*{{{*/
00237     {
00238         sub_.unsubscribe();
00239     }/*}}}*/
00240 
00241     void AmbientSoundDisplay::onEnable()/*{{{*/
00242     {
00243         subscribe();
00244     }/*}}}*/
00245 
00246     void AmbientSoundDisplay::onDisable()/*{{{*/
00247     {
00248         unsubscribe();
00249         clear();
00250     }/*}}}*/
00251 
00252     // When the "Fixed Frame" changes, we need to update our
00253     // tf::MessageFilter and erase existing visuals.
00254     void AmbientSoundDisplay::fixedFrameChanged()/*{{{*/
00255     {
00256         tf_filter_->setTargetFrame( fixed_frame_ );
00257         clear();
00258     }/*}}}*/
00259 
00260     // This is our callback to handle an incoming message.
00261     void AmbientSoundDisplay::incomingMessage( const hark_msgs::HarkPower::ConstPtr& msg )/*{{{*/
00262     {
00263         ++messages_received_;
00264 
00265         // Each display can have multiple status lines.  This one is called
00266         // "Topic" and says how many messages have been received in this case.
00267         std::stringstream ss;
00268         ss << messages_received_ << " messages received";
00269         setStatus( rviz::status_levels::Ok, "Topic", ss.str() );
00270 
00271         // Here we call the rviz::FrameManager to get the transform from the
00272         // fixed frame to the frame in the header of this Imu message.  If
00273         // it fails, we can't do anything else so we return.
00274         Ogre::Quaternion orientation;
00275         Ogre::Vector3 position;
00276         if( !vis_manager_->getFrameManager()->getTransform( msg->header.frame_id,
00277                     msg->header.stamp,
00278                     position, orientation ))
00279         {
00280             ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00281                     msg->header.frame_id.c_str(), fixed_frame_.c_str() );
00282             return;
00283         }
00284 
00285         // We are keeping a circular buffer of visual pointers.  This gets
00286         // the next one, or creates and stores it if it was missing.
00287         AmbientSoundVisual* visual = visuals_[ messages_received_ % history_length_ ];
00288         if( visual == NULL )
00289         {
00290             visual = new AmbientSoundVisual( vis_manager_->getSceneManager(), scene_node_ );
00291             visuals_[ messages_received_ % history_length_ ] = visual;
00292         }
00293 
00294         // Now set or update the contents of the chosen visual.
00295         visual->setFramePosition( position );
00296         visual->setFrameOrientation( orientation );
00297         visual->setColor( color_.r_, color_.g_, color_.b_, alpha_ );
00298         visual->setWidth( width_ );
00299         visual->setScale( scale_ );
00300         visual->setBias ( bias_ );
00301         visual->setGrad ( grad_ );
00302         visual->setMessage( msg );
00303     }/*}}}*/
00304 
00305     // Override rviz::Display's reset() function to add a call to clear().
00306     void AmbientSoundDisplay::reset()/*{{{*/
00307     {
00308         Display::reset();
00309         clear();
00310     }/*}}}*/
00311 
00312     // Override createProperties() to build and configure a Property
00313     // object for each user-editable property.  ``property_manager_``,
00314     // ``property_prefix_``, and ``parent_category_`` are all initialized before
00315     // this is called.
00316     void AmbientSoundDisplay::createProperties()/*{{{*/
00317     {
00318         topic_property_ =
00319             property_manager_->createProperty<rviz::ROSTopicStringProperty>( "Topic",
00320                     property_prefix_,
00321                     boost::bind( &AmbientSoundDisplay::getTopic, this ),
00322                     boost::bind( &AmbientSoundDisplay::setTopic, this, _1 ),
00323                     parent_category_,
00324                     this );
00325         setPropertyHelpText( topic_property_, "hark_msgs::HarkPower topic to subscribe to." );
00326         rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00327         topic_prop->setMessageType( ros::message_traits::datatype<hark_msgs::HarkPower>() );
00328 
00329         color_property_ =
00330             property_manager_->createProperty<rviz::ColorProperty>( "Color",
00331                     property_prefix_,
00332                     boost::bind( &AmbientSoundDisplay::getColor, this ),
00333                     boost::bind( &AmbientSoundDisplay::setColor, this, _1 ),
00334                     parent_category_,
00335                     this );
00336         setPropertyHelpText( color_property_, "Color to draw the acceleration arrows." );
00337 
00338         alpha_property_ =
00339             property_manager_->createProperty<rviz::FloatProperty>( "Alpha",
00340                     property_prefix_,
00341                     boost::bind( &AmbientSoundDisplay::getAlpha, this ),
00342                     boost::bind( &AmbientSoundDisplay::setAlpha, this, _1 ),
00343                     parent_category_,
00344                     this );
00345         setPropertyHelpText( alpha_property_, "0 is fully transparent, 1.0 is fully opaque." );
00346 
00347         history_length_property_ =
00348             property_manager_->createProperty<rviz::IntProperty>( "History Length",
00349                     property_prefix_,
00350                     boost::bind( &AmbientSoundDisplay::getHistoryLength, this ),
00351                     boost::bind( &AmbientSoundDisplay::setHistoryLength, this, _1 ),
00352                     parent_category_,
00353                     this );
00354         setPropertyHelpText( history_length_property_, "Number of prior measurements to display." );
00355         
00356         width_property_ = 
00357             property_manager_->createProperty<rviz::FloatProperty>( "Width",
00358                     property_prefix_,
00359                     boost::bind( &AmbientSoundDisplay::getWidth, this ),
00360                     boost::bind( &AmbientSoundDisplay::setWidth, this, _1 ),
00361                     parent_category_,
00362                     this );
00363         setPropertyHelpText( width_property_, "Width of line" );
00364 
00365         scale_property_ = 
00366             property_manager_->createProperty<rviz::FloatProperty>( "Scale",
00367                     property_prefix_,
00368                     boost::bind( &AmbientSoundDisplay::getScale, this ),
00369                     boost::bind( &AmbientSoundDisplay::setScale, this, _1 ),
00370                     parent_category_,
00371                     this );
00372         setPropertyHelpText( scale_property_, "Scale of line" );
00373         
00374         grad_property_ = 
00375             property_manager_->createProperty<rviz::FloatProperty>( "Grad",
00376                     property_prefix_,
00377                     boost::bind( &AmbientSoundDisplay::getGrad, this ),
00378                     boost::bind( &AmbientSoundDisplay::setGrad, this, _1 ),
00379                     parent_category_,
00380                     this );
00381         setPropertyHelpText( grad_property_, "Graduation" );
00382 
00383         bias_property_ = 
00384             property_manager_->createProperty<rviz::FloatProperty>( "Bias",
00385                     property_prefix_,
00386                     boost::bind( &AmbientSoundDisplay::getBias, this ),
00387                     boost::bind( &AmbientSoundDisplay::setBias, this, _1 ),
00388                     parent_category_,
00389                     this );
00390         setPropertyHelpText( bias_property_, "Bias to subtract" );
00391     }/*}}}*/
00392 
00393 } // end namespace jsk_rviz_plugin
00394 
00395 // Tell pluginlib about this class.  It is important to do this in
00396 // global scope, outside our package's namespace.
00397 #include <pluginlib/class_list_macros.h>
00398 PLUGINLIB_DECLARE_CLASS( jsk_rviz_plugin, AmbientSound, jsk_rviz_plugin::AmbientSoundDisplay, rviz::Display )
00399     // END_TUTORIAL
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


jsk_rviz_plugins
Author(s): Kei Okada
autogenerated on Sat Mar 23 2013 20:30:29