map_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <boost/bind.hpp>
31 
32 #include <OgreManualObject.h>
33 #include <OgreMaterialManager.h>
34 #include <OgreSceneManager.h>
35 #include <OgreSceneNode.h>
36 #include <OgreTextureManager.h>
37 #include <OgreTechnique.h>
38 #include <OgreSharedPtr.h>
39 
40 #include <ros/ros.h>
41 
42 #include <tf/transform_listener.h>
43 
44 #include "rviz/frame_manager.h"
46 #include "rviz/ogre_helpers/grid.h"
54 #include "rviz/validate_floats.h"
56 #include "rviz/display_context.h"
57 
58 #include "map_display.h"
59 
60 namespace rviz
61 {
62 
63 // helper class to set alpha parameter on all renderables.
64 class AlphaSetter: public Ogre::Renderable::Visitor
65 {
66 public:
67  AlphaSetter( float alpha )
68  : alpha_vec_( alpha, alpha, alpha, alpha )
69  {}
70 
71  void visit( Ogre::Renderable *rend, ushort lodIndex, bool isDebug, Ogre::Any *pAny=0)
72  {
73  rend->setCustomParameter( ALPHA_PARAMETER, alpha_vec_ );
74  }
75 private:
76  Ogre::Vector4 alpha_vec_;
77 };
78 
79 
80 Swatch::Swatch(MapDisplay* parent, unsigned int x, unsigned int y, unsigned int width, unsigned int height, float resolution)
81  : parent_(parent), manual_object_( NULL ), x_(x), y_(y), width_(width), height_(height)
82 {
83  // Set up map material
84  static int material_count = 0;
85  std::stringstream ss;
86  ss << "MapMaterial" << material_count++;
87  material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/Indexed8BitImage");
88  material_ = material_->clone( ss.str() );
89 
90  material_->setReceiveShadows(false);
91  material_->getTechnique(0)->setLightingEnabled(false);
92  material_->setDepthBias( -16.0f, 0.0f );
93  material_->setCullingMode( Ogre::CULL_NONE );
94  material_->setDepthWriteEnabled(false);
95 
96  static int map_count = 0;
97  std::stringstream ss2;
98  ss2 << "MapObject" << map_count++;
99  manual_object_ = parent_->scene_manager_->createManualObject( ss2.str() );
100 
101  static int node_count = 0;
102  std::stringstream ss3;
103  ss3 << "NodeObject" << node_count++;
104 
105  scene_node_ = parent_->scene_node_->createChildSceneNode(ss3.str());
106  scene_node_->attachObject( manual_object_ );
107 
108  manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
109  {
110  // First triangle
111  {
112  // Bottom left
113  manual_object_->position( 0.0f, 0.0f, 0.0f );
114  manual_object_->textureCoord(0.0f, 0.0f);
115  manual_object_->normal( 0.0f, 0.0f, 1.0f );
116 
117  // Top right
118  manual_object_->position( 1.0f, 1.0f, 0.0f );
119  manual_object_->textureCoord(1.0f, 1.0f);
120  manual_object_->normal( 0.0f, 0.0f, 1.0f );
121 
122  // Top left
123  manual_object_->position( 0.0f, 1.0f, 0.0f );
124  manual_object_->textureCoord(0.0f, 1.0f);
125  manual_object_->normal( 0.0f, 0.0f, 1.0f );
126  }
127 
128  // Second triangle
129  {
130  // Bottom left
131  manual_object_->position( 0.0f, 0.0f, 0.0f );
132  manual_object_->textureCoord(0.0f, 0.0f);
133  manual_object_->normal( 0.0f, 0.0f, 1.0f );
134 
135  // Bottom right
136  manual_object_->position( 1.0f, 0.0f, 0.0f );
137  manual_object_->textureCoord(1.0f, 0.0f);
138  manual_object_->normal( 0.0f, 0.0f, 1.0f );
139 
140  // Top right
141  manual_object_->position( 1.0f, 1.0f, 0.0f );
142  manual_object_->textureCoord(1.0f, 1.0f);
143  manual_object_->normal( 0.0f, 0.0f, 1.0f );
144  }
145  }
146  manual_object_->end();
147 
148  scene_node_->setPosition( x*resolution, y*resolution, 0 );
149  scene_node_->setScale( width*resolution, height*resolution, 1.0 );
150 
151  if( parent_->draw_under_property_->getValue().toBool() )
152  {
153  manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4);
154  }
155 
156  // don't show map until the plugin is actually enabled
157  manual_object_->setVisible( false );
158 
159 
160 }
161 
163 {
164  parent_->scene_manager_->destroyManualObject(manual_object_);
165 }
166 
167 void Swatch::updateAlpha(const Ogre::SceneBlendType sceneBlending, bool depthWrite, AlphaSetter* alpha_setter)
168 {
169  Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 );
170  Ogre::TextureUnitState* tex_unit = NULL;
171 
172  material_->setSceneBlending( sceneBlending );
173  material_->setDepthWriteEnabled( depthWrite );
174  if( manual_object_ )
175  {
176  manual_object_->visitRenderables( alpha_setter );
177  }
178 }
179 
181 {
182  unsigned int pixels_size = width_ * height_;
183  unsigned char* pixels = new unsigned char[pixels_size];
184  memset(pixels, 255, pixels_size);
185  unsigned char* ptr = pixels;
186  int N = parent_->current_map_.data.size();
187  unsigned int fw = parent_->current_map_.info.width;
188 
189  for(unsigned int yy=y_; yy<y_+height_;yy++){
190  int index = yy * fw + x_;
191  int pixels_to_copy = std::min((int)width_, N-index);
192  memcpy(ptr, &parent_->current_map_.data[ index ], pixels_to_copy);
193  ptr+=pixels_to_copy;
194  if(index+pixels_to_copy>=N) break;
195  }
196 
197  Ogre::DataStreamPtr pixel_stream;
198  pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size ));
199 
200  if( !texture_.isNull() )
201  {
202  Ogre::TextureManager::getSingleton().remove( texture_->getName() );
203  texture_.setNull();
204  }
205 
206  static int tex_count = 0;
207  std::stringstream ss;
208  ss << "MapTexture" << tex_count++;
209  texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
210  pixel_stream, width_, height_, Ogre::PF_L8, Ogre::TEX_TYPE_2D,
211  0);
212 
213  delete[] pixels;
214 }
215 
216 
218  : Display()
219  , loaded_( false )
220  , resolution_( 0.0f )
221  , width_( 0 )
222  , height_( 0 )
223 {
224  connect(this, SIGNAL( mapUpdated() ), this, SLOT( showMap() ));
225  topic_property_ = new RosTopicProperty( "Topic", "",
226  QString::fromStdString( ros::message_traits::datatype<nav_msgs::OccupancyGrid>() ),
227  "nav_msgs::OccupancyGrid topic to subscribe to.",
228  this, SLOT( updateTopic() ));
229 
230  alpha_property_ = new FloatProperty( "Alpha", 0.7,
231  "Amount of transparency to apply to the map.",
232  this, SLOT( updateAlpha() ));
233  alpha_property_->setMin( 0 );
234  alpha_property_->setMax( 1 );
235 
236  color_scheme_property_ = new EnumProperty( "Color Scheme", "map", "How to color the occupancy values.",
237  this, SLOT( updatePalette() ));
238  // Option values here must correspond to indices in palette_textures_ array in onInitialize() below.
239  color_scheme_property_->addOption( "map", 0 );
240  color_scheme_property_->addOption( "costmap", 1 );
241  color_scheme_property_->addOption( "raw", 2 );
242 
243  draw_under_property_ = new Property( "Draw Behind", false,
244  "Rendering option, controls whether or not the map is always"
245  " drawn behind everything else.",
246  this, SLOT( updateDrawUnder() ));
247 
248  resolution_property_ = new FloatProperty( "Resolution", 0,
249  "Resolution of the map. (not editable)", this );
251 
252  width_property_ = new IntProperty( "Width", 0,
253  "Width of the map, in meters. (not editable)", this );
254  width_property_->setReadOnly( true );
255 
256  height_property_ = new IntProperty( "Height", 0,
257  "Height of the map, in meters. (not editable)", this );
258  height_property_->setReadOnly( true );
259 
260  position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
261  "Position of the bottom left corner of the map, in meters. (not editable)",
262  this );
264 
265  orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
266  "Orientation of the map. (not editable)",
267  this );
269 
270  unreliable_property_ = new BoolProperty( "Unreliable", false,
271  "Prefer UDP topic transport",
272  this,
273  SLOT( updateTopic() ));
274 
275  transform_timestamp_property_ = new BoolProperty( "Use Timestamp", false,
276  "Use map header timestamp when transforming",
277  this,
278  SLOT( transformMap() ));
279 }
280 
282 {
283  unsubscribe();
284  clear();
285  for (unsigned i=0; i < swatches.size(); i++){
286  delete swatches[i];
287  }
288  swatches.clear();
289 }
290 
291 unsigned char* makeMapPalette()
292 {
293  unsigned char* palette = new unsigned char[256*4];
294  unsigned char* palette_ptr = palette;
295  // Standard gray map palette values
296  for( int i = 0; i <= 100; i++ )
297  {
298  unsigned char v = 255 - (255 * i) / 100;
299  *palette_ptr++ = v; // red
300  *palette_ptr++ = v; // green
301  *palette_ptr++ = v; // blue
302  *palette_ptr++ = 255; // alpha
303  }
304  // illegal positive values in green
305  for( int i = 101; i <= 127; i++ )
306  {
307  *palette_ptr++ = 0; // red
308  *palette_ptr++ = 255; // green
309  *palette_ptr++ = 0; // blue
310  *palette_ptr++ = 255; // alpha
311  }
312  // illegal negative (char) values in shades of red/yellow
313  for( int i = 128; i <= 254; i++ )
314  {
315  *palette_ptr++ = 255; // red
316  *palette_ptr++ = (255*(i-128))/(254-128); // green
317  *palette_ptr++ = 0; // blue
318  *palette_ptr++ = 255; // alpha
319  }
320  // legal -1 value is tasteful blueish greenish grayish color
321  *palette_ptr++ = 0x70; // red
322  *palette_ptr++ = 0x89; // green
323  *palette_ptr++ = 0x86; // blue
324  *palette_ptr++ = 255; // alpha
325 
326  return palette;
327 }
328 
329 unsigned char* makeCostmapPalette()
330 {
331  unsigned char* palette = new unsigned char[256*4];
332  unsigned char* palette_ptr = palette;
333 
334  // zero values have alpha=0
335  *palette_ptr++ = 0; // red
336  *palette_ptr++ = 0; // green
337  *palette_ptr++ = 0; // blue
338  *palette_ptr++ = 0; // alpha
339 
340  // Blue to red spectrum for most normal cost values
341  for( int i = 1; i <= 98; i++ )
342  {
343  unsigned char v = (255 * i) / 100;
344  *palette_ptr++ = v; // red
345  *palette_ptr++ = 0; // green
346  *palette_ptr++ = 255 - v; // blue
347  *palette_ptr++ = 255; // alpha
348  }
349  // inscribed obstacle values (99) in cyan
350  *palette_ptr++ = 0; // red
351  *palette_ptr++ = 255; // green
352  *palette_ptr++ = 255; // blue
353  *palette_ptr++ = 255; // alpha
354  // lethal obstacle values (100) in purple
355  *palette_ptr++ = 255; // red
356  *palette_ptr++ = 0; // green
357  *palette_ptr++ = 255; // blue
358  *palette_ptr++ = 255; // alpha
359  // illegal positive values in green
360  for( int i = 101; i <= 127; i++ )
361  {
362  *palette_ptr++ = 0; // red
363  *palette_ptr++ = 255; // green
364  *palette_ptr++ = 0; // blue
365  *palette_ptr++ = 255; // alpha
366  }
367  // illegal negative (char) values in shades of red/yellow
368  for( int i = 128; i <= 254; i++ )
369  {
370  *palette_ptr++ = 255; // red
371  *palette_ptr++ = (255*(i-128))/(254-128); // green
372  *palette_ptr++ = 0; // blue
373  *palette_ptr++ = 255; // alpha
374  }
375  // legal -1 value is tasteful blueish greenish grayish color
376  *palette_ptr++ = 0x70; // red
377  *palette_ptr++ = 0x89; // green
378  *palette_ptr++ = 0x86; // blue
379  *palette_ptr++ = 255; // alpha
380 
381  return palette;
382 }
383 
384 unsigned char* makeRawPalette()
385 {
386  unsigned char* palette = new unsigned char[256*4];
387  unsigned char* palette_ptr = palette;
388  // Standard gray map palette values
389  for( int i = 0; i < 256; i++ )
390  {
391  *palette_ptr++ = i; // red
392  *palette_ptr++ = i; // green
393  *palette_ptr++ = i; // blue
394  *palette_ptr++ = 255; // alpha
395  }
396 
397  return palette;
398 }
399 
400 Ogre::TexturePtr makePaletteTexture( unsigned char *palette_bytes )
401 {
402  Ogre::DataStreamPtr palette_stream;
403  palette_stream.bind( new Ogre::MemoryDataStream( palette_bytes, 256*4 ));
404 
405  static int palette_tex_count = 0;
406  std::stringstream ss;
407  ss << "MapPaletteTexture" << palette_tex_count++;
408  return Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
409  palette_stream, 256, 1, Ogre::PF_BYTE_RGBA, Ogre::TEX_TYPE_1D, 0 );
410 }
411 
413 {
414  // Order of palette textures here must match option indices for color_scheme_property_ above.
416  color_scheme_transparency_.push_back( false );
418  color_scheme_transparency_.push_back( true );
420  color_scheme_transparency_.push_back( true );
421 }
422 
424 {
425  subscribe();
426 }
427 
429 {
430  unsubscribe();
431  clear();
432 }
433 
435 {
436  if ( !isEnabled() )
437  {
438  return;
439  }
440 
441  if( !topic_property_->getTopic().isEmpty() )
442  {
443  try
444  {
446  {
448  }else{
450  }
451  setStatus( StatusProperty::Ok, "Topic", "OK" );
452  }
453  catch( ros::Exception& e )
454  {
455  setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
456  }
457 
458  try
459  {
461  setStatus( StatusProperty::Ok, "Update Topic", "OK" );
462  }
463  catch( ros::Exception& e )
464  {
465  setStatus( StatusProperty::Error, "Update Topic", QString( "Error subscribing: " ) + e.what() );
466  }
467  }
468 }
469 
471 {
472  map_sub_.shutdown();
474 }
475 
477 {
478  float alpha = alpha_property_->getFloat();
479  Ogre::SceneBlendType sceneBlending;
480  bool depthWrite;
481 
483  {
484  sceneBlending = Ogre::SBT_TRANSPARENT_ALPHA;
485  depthWrite = false;
486  }
487  else
488  {
489  sceneBlending = Ogre::SBT_REPLACE;
490  depthWrite = !draw_under_property_->getValue().toBool();
491  }
492 
493  AlphaSetter alpha_setter( alpha );
494 
495  for (unsigned i=0; i < swatches.size(); i++) {
496  swatches[i]->updateAlpha(sceneBlending, depthWrite, &alpha_setter);
497  }
498 }
499 
501 {
502  bool draw_under = draw_under_property_->getValue().toBool();
503 
504  if( alpha_property_->getFloat() >= 0.9998 )
505  {
506  for (unsigned i=0; i < swatches.size(); i++)
507  swatches[i]->material_->setDepthWriteEnabled( !draw_under );
508  }
509 
510  int group = draw_under ? Ogre::RENDER_QUEUE_4 : Ogre::RENDER_QUEUE_MAIN;
511  for (unsigned i=0; i < swatches.size(); i++){
512  if( swatches[i]->manual_object_ )
513  swatches[i]->manual_object_->setRenderQueueGroup( group );
514  }
515 }
516 
518 {
519  unsubscribe();
520  subscribe();
521  clear();
522 }
523 
525 {
526  setStatus( StatusProperty::Warn, "Message", "No map received" );
527 
528  if( !loaded_ )
529  {
530  return;
531  }
532 
533  for (unsigned i=0; i < swatches.size(); i++){
534  if( swatches[i]->manual_object_ )
535  swatches[i]->manual_object_->setVisible( false );
536 
537  if( !swatches[i]->texture_.isNull() )
538  {
539  Ogre::TextureManager::getSingleton().remove( swatches[i]->texture_->getName() );
540  swatches[i]->texture_.setNull();
541  }
542  }
543 
544  loaded_ = false;
545 }
546 
547 bool validateFloats(const nav_msgs::OccupancyGrid& msg)
548 {
549  bool valid = true;
550  valid = valid && validateFloats( msg.info.resolution );
551  valid = valid && validateFloats( msg.info.origin );
552  return valid;
553 }
554 
555 void MapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
556 {
557  current_map_ = *msg;
558  // updated via signal in case ros spinner is in a different thread
559  Q_EMIT mapUpdated();
560  loaded_ = true;
561 }
562 
563 
564 void MapDisplay::incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& update)
565 {
566  // Only update the map if we have gotten a full one first.
567  if( !loaded_ )
568  {
569  return;
570  }
571 
572  // Reject updates which have any out-of-bounds data.
573  if( update->x < 0 ||
574  update->y < 0 ||
575  current_map_.info.width < update->x + update->width ||
576  current_map_.info.height < update->y + update->height )
577  {
578  setStatus( StatusProperty::Error, "Update", "Update area outside of original map area." );
579  return;
580  }
581 
582  // Copy the incoming data into current_map_'s data.
583  for( size_t y = 0; y < update->height; y++ )
584  {
585  memcpy( &current_map_.data[ (update->y + y) * current_map_.info.width + update->x ],
586  &update->data[ y * update->width ],
587  update->width );
588  }
589  // updated via signal in case ros spinner is in a different thread
590  Q_EMIT mapUpdated();
591 }
592 
594 {
595  int width = current_map_.info.width;
596  int height = current_map_.info.height;
597  float resolution = current_map_.info.resolution;
598 
599  int sw = width;
600  int sh = height;
601  int n_swatches = 1;
602 
603  for(int i=0;i<4;i++){
604  ROS_INFO("Creating %d swatches", n_swatches);
605  for (unsigned i=0; i < swatches.size(); i++){
606  delete swatches[i];
607  }
608  swatches.clear();
609  try
610  {
611  int x = 0;
612  int y = 0;
613  for(int i=0;i<n_swatches;i++){
614  int tw, th;
615  if(width - x - sw >= sw)
616  tw = sw;
617  else
618  tw = width - x;
619 
620  if(height - y - sh >= sh)
621  th = sh;
622  else
623  th = height - y;
624 
625  swatches.push_back(new Swatch(this, x, y, tw, th, resolution));
626  swatches[i]->updateData();
627 
628  x += tw;
629  if(x>=width){
630  x = 0;
631  y += sh;
632  }
633  }
634  updateAlpha();
635  return;
636  }catch(Ogre::RenderingAPIException&)
637  {
638  ROS_WARN("Failed to create %d swatches", n_swatches);
639  if(sw>sh) sw/=2;
640  else sh/=2;
641  n_swatches *= 2;
642  }
643  }
644 }
645 
647 {
648  if (current_map_.data.empty())
649  {
650  return;
651  }
652 
654  {
655  setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" );
656  return;
657  }
658 
659  if( !validateQuaternions( current_map_.info.origin ))
660  {
661  ROS_WARN_ONCE_NAMED( "quaternions", "Map received on topic '%s' contains unnormalized quaternions. "
662  "This warning will only be output once but may be true for others; "
663  "enable DEBUG messages for ros.rviz.quaternions to see more details.",
664  topic_property_->getTopicStd().c_str() );
665  ROS_DEBUG_NAMED( "quaternions", "Map received on topic '%s' contains unnormalized quaternions.",
666  topic_property_->getTopicStd().c_str() );
667  }
668 
669  if( current_map_.info.width * current_map_.info.height == 0 )
670  {
671  std::stringstream ss;
672  ss << "Map is zero-sized (" << current_map_.info.width << "x" << current_map_.info.height << ")";
673  setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
674  return;
675  }
676 
677  setStatus( StatusProperty::Ok, "Message", "Map received" );
678 
679  ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n",
680  current_map_.info.width,
681  current_map_.info.height,
682  current_map_.info.resolution );
683 
684  float resolution = current_map_.info.resolution;
685 
686  int width = current_map_.info.width;
687  int height = current_map_.info.height;
688 
689  if(width != width_ || height != height_ || resolution_ != resolution){
690  createSwatches();
691  width_ = width;
692  height_ = height;
693  resolution_ = resolution;
694  }
695 
696  Ogre::Vector3 position( current_map_.info.origin.position.x,
697  current_map_.info.origin.position.y,
698  current_map_.info.origin.position.z );
699  Ogre::Quaternion orientation;
700  normalizeQuaternion( current_map_.info.origin.orientation, orientation );
701 
702  frame_ = current_map_.header.frame_id;
703  if (frame_.empty())
704  {
705  frame_ = "/map";
706  }
707 
708  bool map_status_set = false;
709  if( width * height != current_map_.data.size() )
710  {
711  std::stringstream ss;
712  ss << "Data size doesn't match width*height: width = " << width
713  << ", height = " << height << ", data size = " << current_map_.data.size();
714  setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
715  map_status_set = true;
716  }
717 
718  for(int i=0;i<swatches.size();i++){
719  swatches[i]->updateData();
720 
721  Ogre::Pass* pass = swatches[i]->material_->getTechnique(0)->getPass(0);
722  Ogre::TextureUnitState* tex_unit = NULL;
723  if (pass->getNumTextureUnitStates() > 0)
724  {
725  tex_unit = pass->getTextureUnitState(0);
726  }
727  else
728  {
729  tex_unit = pass->createTextureUnitState();
730  }
731 
732  tex_unit->setTextureName(swatches[i]->texture_->getName());
733  tex_unit->setTextureFiltering( Ogre::TFO_NONE );
734  swatches[i]->manual_object_->setVisible( true );
735  }
736 
737 
738  if( !map_status_set )
739  {
740  setStatus( StatusProperty::Ok, "Map", "Map OK" );
741  }
742  updatePalette();
743 
744  resolution_property_->setValue( resolution );
745  width_property_->setValue( width );
746  height_property_->setValue( height );
747  position_property_->setVector( position );
748  orientation_property_->setQuaternion( orientation );
749 
750  transformMap();
751 
753 }
754 
756 {
757  int palette_index = color_scheme_property_->getOptionInt();
758 
759  for (unsigned i=0; i < swatches.size(); i++){
760  Ogre::Pass* pass = swatches[i]->material_->getTechnique(0)->getPass(0);
761  Ogre::TextureUnitState* palette_tex_unit = NULL;
762  if( pass->getNumTextureUnitStates() > 1 )
763  {
764  palette_tex_unit = pass->getTextureUnitState( 1 );
765  }
766  else
767  {
768  palette_tex_unit = pass->createTextureUnitState();
769  }
770  palette_tex_unit->setTextureName( palette_textures_[ palette_index ]->getName() );
771  palette_tex_unit->setTextureFiltering( Ogre::TFO_NONE );
772  }
773 
774  updateAlpha();
775 }
776 
778 {
779  if (!loaded_)
780  {
781  return;
782  }
783 
784  ros::Time transform_time;
785 
787  {
788  transform_time = current_map_.header.stamp;
789  }
790 
791  Ogre::Vector3 position;
792  Ogre::Quaternion orientation;
793  if (!context_->getFrameManager()->transform(frame_, transform_time, current_map_.info.origin, position, orientation) &&
794  !context_->getFrameManager()->transform(frame_, ros::Time(0), current_map_.info.origin, position, orientation))
795  {
796  ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'",
797  qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ ));
798 
799  setStatus( StatusProperty::Error, "Transform",
800  "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" );
801  }
802  else
803  {
804  setStatus(StatusProperty::Ok, "Transform", "Transform OK");
805  }
806 
807  scene_node_->setPosition( position );
808  scene_node_->setOrientation( orientation );
809 }
810 
812 {
813  transformMap();
814 }
815 
817 {
818  Display::reset();
819 
820  clear();
821  // Force resubscription so that the map will be re-sent
822  updateTopic();
823 }
824 
825 void MapDisplay::setTopic( const QString &topic, const QString &datatype )
826 {
827  topic_property_->setString( topic );
828 }
829 
830 void MapDisplay::update( float wall_dt, float ros_dt ) {
831  transformMap();
832 }
833 
834 } // namespace rviz
835 
#define ALPHA_PARAMETER
#define NULL
Definition: global.h:37
void setMin(float min)
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
void updateData()
std::string frame_
Definition: map_display.h:152
unsigned char * makeCostmapPalette()
Ogre::TexturePtr makePaletteTexture(unsigned char *palette_bytes)
virtual void setString(const QString &str)
void setMax(float max)
unsigned char * makeMapPalette()
Ogre::ManualObject * manual_object_
Definition: map_display.h:80
TransportHints & reliable()
virtual bool setVector(const Ogre::Vector3 &vector)
virtual void reset()
Called to tell the display to clear its state.
f
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
virtual void setTopic(const QString &topic, const QString &datatype)
Set the ROS topic to listen to for this display.
std::vector< Ogre::TexturePtr > palette_textures_
Definition: map_display.h:144
QuaternionProperty * orientation_property_
Definition: map_display.h:163
virtual void subscribe()
std::vector< bool > color_scheme_transparency_
Definition: map_display.h:145
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:269
Ogre::TexturePtr texture_
Definition: map_display.h:81
MapDisplay * parent_
Definition: map_display.h:79
virtual void onInitialize()
Override this function to do subclass-specific initialization.
FloatProperty * alpha_property_
Definition: map_display.h:164
virtual float getFloat() const
void incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr &update)
Copy update&#39;s data into current_map_ and call showMap().
unsigned char * makeRawPalette()
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
Ogre::MaterialPtr material_
Definition: map_display.h:82
Property specialized to enforce floating point max/min.
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
AlphaSetter(float alpha)
Definition: map_display.cpp:67
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:38
ros::Subscriber update_sub_
Definition: map_display.h:156
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
VectorProperty * position_property_
Definition: map_display.h:162
FloatProperty * resolution_property_
Definition: map_display.h:159
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:281
virtual bool getBool() const
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
unsigned int x_
Definition: map_display.h:84
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
#define ROS_DEBUG_NAMED(name,...)
unsigned int y_
Definition: map_display.h:84
TransportHints & unreliable()
virtual void unsubscribe()
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:300
#define ROS_INFO(...)
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
std::vector< Swatch * > swatches
Definition: map_display.h:143
virtual void onDisable()
Derived classes override this to do the actual work of disabling themselves.
void showMap()
Show current_map_ in the scene.
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
Ogre::Vector4 alpha_vec_
Definition: map_display.cpp:76
virtual bool setQuaternion(const Ogre::Quaternion &quaternion)
void updateAlpha(const Ogre::SceneBlendType sceneBlending, bool depthWrite, AlphaSetter *alpha_setter)
virtual ~MapDisplay()
void incomingMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
Copy msg into current_map_ and call showMap().
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Transform a pose from a frame into the fixed frame.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
EnumProperty * color_scheme_property_
Definition: map_display.h:166
TFSIMD_FORCE_INLINE const tfScalar & x() const
Displays a map along the XY plane.
Definition: map_display.h:92
RosTopicProperty * topic_property_
Definition: map_display.h:158
unsigned int height_
Definition: map_display.h:84
IntProperty * height_property_
Definition: map_display.h:161
friend class Swatch
Definition: map_display.h:94
float normalizeQuaternion(float &w, float &x, float &y, float &z)
#define ROS_WARN_ONCE_NAMED(name,...)
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Definition: display.h:261
Property * draw_under_property_
Definition: map_display.h:165
BoolProperty * unreliable_property_
Definition: map_display.h:168
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
ros::Subscriber map_sub_
Definition: map_display.h:155
BoolProperty * transform_timestamp_property_
Definition: map_display.h:169
QString getTopic() const
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
Constructor.
Definition: property.cpp:54
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
unsigned int width_
Definition: map_display.h:84
virtual QVariant getValue() const
Return the value of this Property as a QVariant. If the value has never been set, an invalid QVariant...
Definition: property.cpp:145
Ogre::SceneNode * scene_node_
Definition: map_display.h:83
IntProperty * width_property_
Definition: map_display.h:160
std::string getTopicStd() const
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:397
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void visit(Ogre::Renderable *rend, ushort lodIndex, bool isDebug, Ogre::Any *pAny=0)
Definition: map_display.cpp:71
Enum property.
Definition: enum_property.h:47
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:186
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:159
nav_msgs::OccupancyGrid current_map_
Definition: map_display.h:153
Swatch(MapDisplay *parent, unsigned int x, unsigned int y, unsigned int width, unsigned int height, float resolution)
Definition: map_display.cpp:80
void mapUpdated()
Emitted when a new map is received.
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51