ambient_sound_display_groovy.cpp
Go to the documentation of this file.
1 #include <OGRE/OgreSceneNode.h>
2 #include <OGRE/OgreSceneManager.h>
3 
5 
11 #include <rviz/frame_manager.h>
12 #include <rviz/validate_floats.h>
13 
14 #include <boost/foreach.hpp>
15 #include "ambient_sound_visual.h"
17 
18 namespace jsk_rviz_plugins
19 {
20 
22  {
23  color_property_ = new rviz::ColorProperty("Color",QColor( 204, 51, 204),
24  "Color to draw the acceleration arrows." ,
25  this, SLOT(updateColorAndAlpha()));
26  alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
27  "0 is fully transparent, 1.0 is fully opaque.",
28  this, SLOT( updateColorAndAlpha() ));
29  history_length_property_ = new rviz::IntProperty("History Length", 1,
30  "Number of prior measurements to display." ,
31  this, SLOT(updateHistoryLength()));
32  width_property_ = new rviz::FloatProperty("Width", 0.1,
33  "Width of line",
34  this, SLOT(updateAppearance()));
35  scale_property_ = new rviz::FloatProperty("Scale", 1.0,
36  "Scale of line",
37  this, SLOT(updateAppearance()));
38  bias_property_ = new rviz::FloatProperty("Bias", 10,
39  "Bias",
40  this, SLOT(updateAppearance()));
41  grad_property_ = new rviz::FloatProperty("Gradient", 0.1,
42  "Gradient",
43  this, SLOT(updateAppearance()));
44  history_length_property_->setMin( 1 );
45  history_length_property_->setMax( 1 );
46  }/*}}}*/
47 
48  // After the parent rviz::Display::initialize() does its own setup, it
49  // calls the subclass's onInitialize() function. This is where we
50  // instantiate all the workings of the class.
52  {
54  // Make an Ogre::SceneNode to contain all our visuals.
55  //scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
56 
57  // Set the default history length and resize the ``visuals_`` array.
58  //setHistoryLength( 1 );
60 
61  // A tf::MessageFilter listens to ROS messages and calls our
62  // callback with them when they can be matched up with valid tf
63  // transform data.
64 // tf_filter_ =
65 // new tf::MessageFilter<jsk_hark_msgs::HarkPower>( *context_->getTFClient(),
66 // "", 100, update_nh_ );
67 // tf_filter_->connectInput( sub_ );
68 // tf_filter_->registerCallback( boost::bind( &AmbientSoundDisplay::incomingMessage,
69 // this, _1 ));
70 
71  // FrameManager has some built-in functions to set the status of a
72  // Display based on callbacks from a tf::MessageFilter. These work
73  // fine for this simple display.
74 // vis_manager_->getFrameManager()
75 // ->registerFilterForTransformStatusCheck( tf_filter_, this );
76  }/*}}}*/
77 
79  {
80 // for( size_t i = 0; i < visuals_.size(); i++ )
81 // {
82 // delete visuals_[ i ];
83 // }
84 //
85 // delete tf_filter_;
86  }/*}}}*/
87 
88  // Clear the visuals by deleting their objects.
90  {
92  visuals_.clear();
93  }
94 
95 /*{{{*/
96 // void AmbientSoundDisplay::clear()
97 // {
98 // for( size_t i = 0; i < visuals_.size(); i++ )
99 // {
100 // delete visuals_[ i ];
101 // visuals_[ i ] = NULL;
102 // }
103 // tf_filter_->clear();
104 // messages_received_ = 0;
105 // setStatus( rviz::status_levels::Warn, "Topic", "No messages received" );
106 // }/*}}}*/
107 
108  // Set the current color and alpha values for each visual.
110  {
111  float alpha = alpha_property_->getFloat();
112  Ogre::ColourValue color = color_property_->getOgreColor();
113 
114  for( size_t i = 0; i < visuals_.size(); i++ )
115  {
116  if( visuals_[ i ] )
117  {
118  visuals_[ i ]->setColor( color.r, color.g, color.b, alpha );
119  }
120  }
121  }/*}}}*/
122 
123  // Set the appearance of the line
125  float width = width_property_->getFloat();
126  float scale = scale_property_->getFloat();
127  float bias = bias_property_->getFloat();
128  float grad = grad_property_->getFloat();
129 
130  for( size_t i = 0; i < visuals_.size(); i++ )
131  {
132  if( visuals_[ i ] )
133  {
134  visuals_[ i ]->setWidth( width );
135  visuals_[ i ]->setScale( scale );
136  visuals_[ i ]->setBias ( bias );
137  visuals_[ i ]->setGrad ( grad );
138  }
139  }
140  }/*}}}*/
141 
142  // Set the number of past visuals to show.
144  visuals_.rset_capacity(history_length_property_->getInt());
145  }/*}}}*/
146 
147  bool AmbientSoundDisplay::validateFloats( const jsk_hark_msgs::HarkPower& msg )
148  {
149  std::vector<float>::const_iterator it = msg.powers.begin();
150  for (; it < msg.powers.end(); ++it) {
151  if(!rviz::validateFloats(*it)){
152  return false;
153  };
154  }
155  return true;
156  }
157 
158  //void AmbientSoundDisplay::setHistoryLength( int length )[>{{{<]
159  //{
161  //if( length < 1 )
162  //{
163  //length = 1;
164  //}
166  //if( history_length_ == length )
167  //{
168  //return;
169  //}
170 
172  //history_length_ = length;
173  //propertyChanged( history_length_property_ );
174 
176  //std::vector<AmbientSoundVisual*> new_visuals( history_length_, (AmbientSoundVisual*)0 );
177 
180  //size_t copy_len =
181  //(new_visuals.size() > visuals_.size()) ?
182  //visuals_.size() : new_visuals.size();
183  //for( size_t i = 0; i < copy_len; i++ )
184  //{
185  //int new_index = (messages_received_ - i) % new_visuals.size();
186  //int old_index = (messages_received_ - i) % visuals_.size();
187  //new_visuals[ new_index ] = visuals_[ old_index ];
188  //visuals_[ old_index ] = NULL;
189  //}
190 
192  //for( size_t i = 0; i < visuals_.size(); i++ ) {
193  //delete visuals_[ i ];
194  //}
195 
198 
201  //visuals_.swap( new_visuals );
202  //}[>}}}<]
203 
204  // When the "Fixed Frame" changes, we need to update our
205  // tf::MessageFilter and erase existing visuals.
206  //void AmbientSoundDisplay::fixedFrameChanged()[>{{{<]
207  //{
208  //tf_filter_->setTargetFrame( fixed_frame_ );
209  //clear();
210  //}[>}}}<]
211 
212  // This is our callback to handle an incoming message.
213  void AmbientSoundDisplay::processMessage( const jsk_hark_msgs::HarkPower::ConstPtr& msg )/*{{{*/
214  {
215  if( !validateFloats( *msg ))
216  {
217  setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
218  return;
219  }
220 
221  // Here we call the rviz::FrameManager to get the transform from the
222  // fixed frame to the frame in the header of this Imu message. If
223  // it fails, we can't do anything else so we return.
224  Ogre::Quaternion orientation;
225  Ogre::Vector3 position;
226  if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
227  msg->header.stamp,
228  position, orientation ))
229  {
230  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
231  msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
232  return;
233  }
234 
235  // We are keeping a circular buffer of visual pointers. This gets
236  // the next one, or creates and stores it if it was missing.
237 #if ROS_VERSION_MINIMUM(1,12,0)
238  std::shared_ptr<AmbientSoundVisual> visual;
239 #else
241 #endif
242  //AmbientSoundVisual* visual_ptr;
243  if( visuals_.full())
244  {
245  visual = visuals_.front();
246  //visual.reset(visuals_.front());
247  //visual_ptr = visuals_.front();
248  //visual.reset(visual_ptr);
249  } else {
251  }
252 
253  // Now set or update the contents of the chosen visual.
254  visual->setMessage( msg );
255  visual->setFramePosition( position );
256  visual->setFrameOrientation( orientation );
257 
258  float alpha = alpha_property_->getFloat();
259  Ogre::ColourValue color = color_property_->getOgreColor();
260  float width = width_property_->getFloat();
261  float scale = scale_property_->getFloat();
262  float bias = bias_property_->getFloat();
263  float grad = grad_property_->getFloat();
264 
265  visual->setColor( color.r, color.g, color.b, alpha );
266  visual->setWidth( width );
267  visual->setScale( scale );
268  visual->setBias ( bias );
269  visual->setGrad ( grad );
270 
271  visuals_.push_back(visual);
272  }/*}}}*/
273 
274 } // end namespace jsk_rviz_plugin
275 
276 // Tell pluginlib about this class. It is important to do this in
277 // global scope, outside our package's namespace.
280 
bool validateFloats(const jsk_hark_msgs::HarkPower &)
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
width
QString fixed_frame_
virtual FrameManager * getFrameManager() const =0
bool validateFloats(const visualization_msgs::InteractiveMarker &msg)
std::vector< AmbientSoundVisual * > visuals_
void processMessage(const jsk_hark_msgs::HarkPower::ConstPtr &msg)
virtual Ogre::SceneManager * getSceneManager() const =0
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
#define ROS_DEBUG(...)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18