ambient_sound_display_groovy.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/color_property.h>
00008 #include <rviz/properties/float_property.h>
00009 #include <rviz/properties/int_property.h>
00010 #include <rviz/properties/ros_topic_property.h>
00011 #include <rviz/frame_manager.h>
00012 #include <rviz/validate_floats.h>
00013 
00014 #include <boost/foreach.hpp>
00015 #include "ambient_sound_visual.h"
00016 #include "ambient_sound_display_groovy.h"
00017 
00018 namespace jsk_rviz_plugins
00019 {
00020 
00021     AmbientSoundDisplay::AmbientSoundDisplay()/*{{{*/
00022     {
00023           color_property_ = new rviz::ColorProperty("Color",QColor( 204, 51, 204),
00024                   "Color to draw the acceleration arrows." ,
00025                   this, SLOT(updateColorAndAlpha()));
00026           alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
00027                   "0 is fully transparent, 1.0 is fully opaque.",
00028                   this, SLOT( updateColorAndAlpha() ));
00029           history_length_property_ = new rviz::IntProperty("History Length", 1,
00030                   "Number of prior measurements to display." ,
00031                   this, SLOT(updateHistoryLength()));
00032           width_property_ = new rviz::FloatProperty("Width", 0.1,
00033                   "Width of line",
00034                   this, SLOT(updateAppearance()));
00035           scale_property_ = new rviz::FloatProperty("Scale", 1.0,
00036                   "Scale of line",
00037                   this, SLOT(updateAppearance()));
00038           bias_property_ = new rviz::FloatProperty("Bias", 10,
00039                   "Bias",
00040                   this, SLOT(updateAppearance()));
00041           grad_property_ = new rviz::FloatProperty("Gradient", 0.1,
00042                   "Gradient",
00043                   this, SLOT(updateAppearance()));
00044           history_length_property_->setMin( 1 );
00045           history_length_property_->setMax( 1 );
00046     }/*}}}*/
00047 
00048     // After the parent rviz::Display::initialize() does its own setup, it
00049     // calls the subclass's onInitialize() function.  This is where we
00050     // instantiate all the workings of the class.
00051     void AmbientSoundDisplay::onInitialize()/*{{{*/
00052     {
00053         MFDClass::onInitialize();
00054         // Make an Ogre::SceneNode to contain all our visuals.
00055         //scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00056 
00057         // Set the default history length and resize the ``visuals_`` array.
00058         //setHistoryLength( 1 );
00059         updateHistoryLength();
00060 
00061         // A tf::MessageFilter listens to ROS messages and calls our
00062         // callback with them when they can be matched up with valid tf
00063         // transform data.
00064 //        tf_filter_ =
00065 //            new tf::MessageFilter<jsk_hark_msgs::HarkPower>( *context_->getTFClient(),
00066 //                    "", 100, update_nh_ );
00067 //        tf_filter_->connectInput( sub_ );
00068 //        tf_filter_->registerCallback( boost::bind( &AmbientSoundDisplay::incomingMessage,
00069 //                    this, _1 ));
00070 
00071         // FrameManager has some built-in functions to set the status of a
00072         // Display based on callbacks from a tf::MessageFilter.  These work
00073         // fine for this simple display.
00074 //        vis_manager_->getFrameManager()
00075 //            ->registerFilterForTransformStatusCheck( tf_filter_, this );
00076     }/*}}}*/
00077 
00078     AmbientSoundDisplay::~AmbientSoundDisplay()/*{{{*/
00079     {
00080 //        for( size_t i = 0; i < visuals_.size(); i++ )
00081 //        {
00082 //            delete visuals_[ i ];
00083 //        }
00084 //
00085 //        delete tf_filter_;
00086     }/*}}}*/
00087 
00088     // Clear the visuals by deleting their objects.
00089     void AmbientSoundDisplay::reset()
00090     {
00091         MFDClass::reset();
00092         visuals_.clear();
00093     }
00094 
00095 /*{{{*/
00096 //    void AmbientSoundDisplay::clear()
00097 //    {
00098 //        for( size_t i = 0; i < visuals_.size(); i++ )
00099 //        {
00100 //            delete visuals_[ i ];
00101 //            visuals_[ i ] = NULL;
00102 //        }
00103 //        tf_filter_->clear();
00104 //        messages_received_ = 0;
00105 //        setStatus( rviz::status_levels::Warn, "Topic", "No messages received" );
00106 //    }/*}}}*/
00107 
00108     // Set the current color and alpha values for each visual.
00109     void AmbientSoundDisplay::updateColorAndAlpha()/*{{{*/
00110     {
00111         float alpha = alpha_property_->getFloat();
00112         Ogre::ColourValue color = color_property_->getOgreColor();
00113 
00114         for( size_t i = 0; i < visuals_.size(); i++ )
00115         {
00116             if( visuals_[ i ] )
00117             {
00118                 visuals_[ i ]->setColor( color.r, color.g, color.b, alpha );
00119             }
00120         }
00121     }/*}}}*/
00122 
00123     // Set the appearance of the line
00124     void AmbientSoundDisplay::updateAppearance(){/*{{{*/
00125         float width = width_property_->getFloat();
00126         float scale = scale_property_->getFloat();
00127         float bias = bias_property_->getFloat();
00128         float grad = grad_property_->getFloat();
00129 
00130         for( size_t i = 0; i < visuals_.size(); i++ )
00131         {
00132             if( visuals_[ i ] )
00133             {
00134                 visuals_[ i ]->setWidth( width );
00135                 visuals_[ i ]->setScale( scale );
00136                 visuals_[ i ]->setBias ( bias );
00137                 visuals_[ i ]->setGrad ( grad );
00138             }
00139         }
00140     }/*}}}*/
00141 
00142     // Set the number of past visuals to show.
00143     void AmbientSoundDisplay::updateHistoryLength(){/*{{{*/
00144         visuals_.rset_capacity(history_length_property_->getInt());
00145     }/*}}}*/
00146 
00147     bool AmbientSoundDisplay::validateFloats( const jsk_hark_msgs::HarkPower& msg )
00148     {
00149         std::vector<float>::const_iterator it = msg.powers.begin();
00150         for (; it < msg.powers.end(); ++it) {
00151             if(!rviz::validateFloats(*it)){
00152                 return false;
00153             };
00154         }        
00155         return true;
00156     }
00157 
00158     //void AmbientSoundDisplay::setHistoryLength( int length )[>{{{<]
00159     //{
00161         //if( length < 1 )
00162         //{
00163             //length = 1;
00164         //}
00166         //if( history_length_ == length )
00167         //{
00168             //return;
00169         //}
00170 
00172         //history_length_ = length;
00173         //propertyChanged( history_length_property_ );
00174 
00176         //std::vector<AmbientSoundVisual*> new_visuals( history_length_, (AmbientSoundVisual*)0 );
00177 
00180         //size_t copy_len =
00181             //(new_visuals.size() > visuals_.size()) ?
00182             //visuals_.size() : new_visuals.size();
00183         //for( size_t i = 0; i < copy_len; i++ )
00184         //{
00185             //int new_index = (messages_received_ - i) % new_visuals.size();
00186             //int old_index = (messages_received_ - i) % visuals_.size();
00187             //new_visuals[ new_index ] = visuals_[ old_index ];
00188             //visuals_[ old_index ] = NULL;
00189         //}
00190 
00192         //for( size_t i = 0; i < visuals_.size(); i++ ) {
00193             //delete visuals_[ i ];
00194         //}
00195 
00198 
00201         //visuals_.swap( new_visuals );
00202     //}[>}}}<]
00203 
00204     // When the "Fixed Frame" changes, we need to update our
00205     // tf::MessageFilter and erase existing visuals.
00206     //void AmbientSoundDisplay::fixedFrameChanged()[>{{{<]
00207     //{
00208         //tf_filter_->setTargetFrame( fixed_frame_ );
00209         //clear();
00210     //}[>}}}<]
00211 
00212     // This is our callback to handle an incoming message.
00213     void AmbientSoundDisplay::processMessage( const jsk_hark_msgs::HarkPower::ConstPtr& msg )/*{{{*/
00214     {
00215         if( !validateFloats( *msg ))
00216         {
00217             setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00218             return;
00219         }
00220 
00221         // Here we call the rviz::FrameManager to get the transform from the
00222         // fixed frame to the frame in the header of this Imu message.  If
00223         // it fails, we can't do anything else so we return.
00224         Ogre::Quaternion orientation;
00225         Ogre::Vector3 position;
00226         if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
00227                     msg->header.stamp,
00228                     position, orientation ))
00229         {
00230             ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00231                     msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00232             return;
00233         }
00234 
00235         // We are keeping a circular buffer of visual pointers.  This gets
00236         // the next one, or creates and stores it if it was missing.
00237         boost::shared_ptr<AmbientSoundVisual> visual;
00238         //AmbientSoundVisual* visual_ptr;
00239         if( visuals_.full())
00240         {
00241             visual =  visuals_.front();
00242             //visual.reset(visuals_.front());
00243             //visual_ptr = visuals_.front();
00244             //visual.reset(visual_ptr);
00245         } else {
00246             visual.reset(new AmbientSoundVisual(context_->getSceneManager(), scene_node_));
00247         }
00248 
00249         // Now set or update the contents of the chosen visual.
00250         visual->setMessage( msg );
00251         visual->setFramePosition( position );
00252         visual->setFrameOrientation( orientation );
00253 
00254         float alpha = alpha_property_->getFloat();
00255         Ogre::ColourValue color = color_property_->getOgreColor();
00256         float width = width_property_->getFloat();
00257         float scale = scale_property_->getFloat();
00258         float bias = bias_property_->getFloat();
00259         float grad = grad_property_->getFloat();
00260 
00261         visual->setColor( color.r, color.g, color.b, alpha );
00262         visual->setWidth( width );
00263         visual->setScale( scale );
00264         visual->setBias ( bias );
00265         visual->setGrad ( grad );
00266 
00267         visuals_.push_back(visual);
00268     }/*}}}*/
00269 
00270 } // end namespace jsk_rviz_plugin
00271 
00272 // Tell pluginlib about this class.  It is important to do this in
00273 // global scope, outside our package's namespace.
00274 #include <pluginlib/class_list_macros.h>
00275 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::AmbientSoundDisplay, rviz::Display )
00276 


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03