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/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 <rviz/frame_manager.h>
44 #include <rviz/ogre_helpers/grid.h>
52 #include <rviz/validate_floats.h>
54 #include <rviz/display_context.h>
55 
56 #include "map_display.h"
57 
58 namespace rviz
59 {
60 // helper class to set alpha parameter on all renderables.
61 class AlphaSetter : public Ogre::Renderable::Visitor
62 {
63 public:
64  AlphaSetter(float alpha) : alpha_vec_(alpha, alpha, alpha, alpha)
65  {
66  }
67 
68  void visit(Ogre::Renderable* rend,
69  ushort /*lodIndex*/,
70  bool /*isDebug*/,
71  Ogre::Any* /*pAny*/ = nullptr) override
72  {
73  rend->setCustomParameter(ALPHA_PARAMETER, alpha_vec_);
74  }
75 
76 private:
77  Ogre::Vector4 alpha_vec_;
78 };
79 
80 
82  unsigned int x,
83  unsigned int y,
84  unsigned int width,
85  unsigned int height,
86  float resolution)
87  : parent_(parent), manual_object_(nullptr), x_(x), y_(y), width_(width), height_(height)
88 {
89  // Set up map material
90  static int material_count = 0;
91  std::stringstream ss;
92  ss << "MapMaterial" << material_count++;
93  material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/Indexed8BitImage");
94  material_ = material_->clone(ss.str());
95 
96  material_->setReceiveShadows(false);
97  material_->getTechnique(0)->setLightingEnabled(false);
98  material_->setDepthBias(-16.0f, 0.0f);
99  material_->setCullingMode(Ogre::CULL_NONE);
100  material_->setDepthWriteEnabled(false);
101 
102  static int map_count = 0;
103  std::stringstream ss2;
104  ss2 << "MapObject" << map_count++;
105  manual_object_ = parent_->scene_manager_->createManualObject(ss2.str());
106 
107  static int node_count = 0;
108  std::stringstream ss3;
109  ss3 << "NodeObject" << node_count++;
110 
111  scene_node_ = parent_->scene_node_->createChildSceneNode(ss3.str());
112  scene_node_->attachObject(manual_object_);
113 
114  manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
115  {
116  // First triangle
117  {
118  // Bottom left
119  manual_object_->position(0.0f, 0.0f, 0.0f);
120  manual_object_->textureCoord(0.0f, 0.0f);
121  manual_object_->normal(0.0f, 0.0f, 1.0f);
122 
123  // Top right
124  manual_object_->position(1.0f, 1.0f, 0.0f);
125  manual_object_->textureCoord(1.0f, 1.0f);
126  manual_object_->normal(0.0f, 0.0f, 1.0f);
127 
128  // Top left
129  manual_object_->position(0.0f, 1.0f, 0.0f);
130  manual_object_->textureCoord(0.0f, 1.0f);
131  manual_object_->normal(0.0f, 0.0f, 1.0f);
132  }
133 
134  // Second triangle
135  {
136  // Bottom left
137  manual_object_->position(0.0f, 0.0f, 0.0f);
138  manual_object_->textureCoord(0.0f, 0.0f);
139  manual_object_->normal(0.0f, 0.0f, 1.0f);
140 
141  // Bottom right
142  manual_object_->position(1.0f, 0.0f, 0.0f);
143  manual_object_->textureCoord(1.0f, 0.0f);
144  manual_object_->normal(0.0f, 0.0f, 1.0f);
145 
146  // Top right
147  manual_object_->position(1.0f, 1.0f, 0.0f);
148  manual_object_->textureCoord(1.0f, 1.0f);
149  manual_object_->normal(0.0f, 0.0f, 1.0f);
150  }
151  }
152  manual_object_->end();
153 
154  scene_node_->setPosition(x * resolution, y * resolution, 0);
155  scene_node_->setScale(width * resolution, height * resolution, 1.0);
156 
157  if (parent_->draw_under_property_->getValue().toBool())
158  {
159  manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4);
160  }
161 
162  // don't show map until the plugin is actually enabled
163  manual_object_->setVisible(false);
164 }
165 
167 {
168  parent_->scene_manager_->destroyManualObject(manual_object_);
169 }
170 
171 void Swatch::updateAlpha(const Ogre::SceneBlendType sceneBlending,
172  bool depthWrite,
173  AlphaSetter* alpha_setter)
174 {
175  material_->setSceneBlending(sceneBlending);
176  material_->setDepthWriteEnabled(depthWrite);
177  if (manual_object_)
178  {
179  manual_object_->visitRenderables(alpha_setter);
180  }
181 }
182 
184 {
185  unsigned int pixels_size = width_ * height_;
186  unsigned char* pixels = new unsigned char[pixels_size];
187  memset(pixels, 255, pixels_size);
188  unsigned char* ptr = pixels;
189  int N = parent_->current_map_.data.size();
190  unsigned int fw = parent_->current_map_.info.width;
191 
192  for (unsigned int yy = y_; yy < y_ + height_; yy++)
193  {
194  int index = yy * fw + x_;
195  int pixels_to_copy = std::min((int)width_, N - index);
196  memcpy(ptr, &parent_->current_map_.data[index], pixels_to_copy);
197  ptr += pixels_to_copy;
198  if (index + pixels_to_copy >= N)
199  break;
200  }
201 
202  Ogre::DataStreamPtr pixel_stream;
203  pixel_stream.bind(new Ogre::MemoryDataStream(pixels, pixels_size));
204 
205  if (!texture_.isNull())
206  {
207  Ogre::TextureManager::getSingleton().remove(texture_->getName());
208  texture_.setNull();
209  }
210 
211  static int tex_count = 0;
212  std::stringstream ss;
213  ss << "MapTexture" << tex_count++;
214  texture_ = Ogre::TextureManager::getSingleton().loadRawData(
215  ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, width_, height_,
216  Ogre::PF_L8, Ogre::TEX_TYPE_2D, 0);
217 
218  delete[] pixels;
219 }
220 
221 
223  : Display(), loaded_(false), map_updated_(false), resolution_(0.0f), width_(0), height_(0)
224 {
226  "Topic", "", QString::fromStdString(ros::message_traits::datatype<nav_msgs::OccupancyGrid>()),
227  "nav_msgs::OccupancyGrid topic to subscribe to.", this, &MapDisplay::updateTopic);
228 
229  alpha_property_ = new FloatProperty("Alpha", 0.7, "Amount of transparency to apply to the map.", this,
233 
234  color_scheme_property_ = new EnumProperty("Color Scheme", "map", "How to color the occupancy values.",
236  // Option values here must correspond to indices in palette_textures_ array in onInitialize() below.
238  color_scheme_property_->addOption("costmap", 1);
240 
242  "Draw Behind", false,
243  "Rendering option, controls whether or not the map is always drawn behind everything else.", this,
245 
247  new FloatProperty("Resolution", 0, "Resolution of the map. (not editable)", this);
249 
250  width_property_ = new IntProperty("Width", 0, "Width of the map, in meters. (not editable)", this);
252 
253  height_property_ = new IntProperty("Height", 0, "Height of the map, in meters. (not editable)", this);
255 
257  new VectorProperty("Position", Ogre::Vector3::ZERO,
258  "Position of the bottom left corner of the map, in meters. (not editable)",
259  this);
261 
262  orientation_property_ = new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY,
263  "Orientation of the map. (not editable)", this);
265 
266  unreliable_property_ = new BoolProperty("Unreliable", false, "Prefer UDP topic transport", this,
268 
270  new BoolProperty("Use Timestamp", false, "Use map header timestamp when transforming", this,
272 }
273 
275 {
277  clear();
278  for (unsigned i = 0; i < swatches.size(); i++)
279  {
280  delete swatches[i];
281  }
282  swatches.clear();
283 }
284 
285 unsigned char* makeMapPalette()
286 {
287  unsigned char* palette = OGRE_ALLOC_T(unsigned char, 256 * 4, Ogre::MEMCATEGORY_GENERAL);
288  unsigned char* palette_ptr = palette;
289  // Standard gray map palette values
290  for (int i = 0; i <= 100; i++)
291  {
292  unsigned char v = 255 - (255 * i) / 100;
293  *palette_ptr++ = v; // red
294  *palette_ptr++ = v; // green
295  *palette_ptr++ = v; // blue
296  *palette_ptr++ = 255; // alpha
297  }
298  // illegal positive values in green
299  for (int i = 101; i <= 127; i++)
300  {
301  *palette_ptr++ = 0; // red
302  *palette_ptr++ = 255; // green
303  *palette_ptr++ = 0; // blue
304  *palette_ptr++ = 255; // alpha
305  }
306  // illegal negative (char) values in shades of red/yellow
307  for (int i = 128; i <= 254; i++)
308  {
309  *palette_ptr++ = 255; // red
310  *palette_ptr++ = (255 * (i - 128)) / (254 - 128); // green
311  *palette_ptr++ = 0; // blue
312  *palette_ptr++ = 255; // alpha
313  }
314  // legal -1 value is tasteful blueish greenish grayish color
315  *palette_ptr++ = 0x70; // red
316  *palette_ptr++ = 0x89; // green
317  *palette_ptr++ = 0x86; // blue
318  *palette_ptr++ = 255; // alpha
319 
320  return palette;
321 }
322 
323 unsigned char* makeCostmapPalette()
324 {
325  unsigned char* palette = OGRE_ALLOC_T(unsigned char, 256 * 4, Ogre::MEMCATEGORY_GENERAL);
326  unsigned char* palette_ptr = palette;
327 
328  // zero values have alpha=0
329  *palette_ptr++ = 0; // red
330  *palette_ptr++ = 0; // green
331  *palette_ptr++ = 0; // blue
332  *palette_ptr++ = 0; // alpha
333 
334  // Blue to red spectrum for most normal cost values
335  for (int i = 1; i <= 98; i++)
336  {
337  unsigned char v = (255 * i) / 100;
338  *palette_ptr++ = v; // red
339  *palette_ptr++ = 0; // green
340  *palette_ptr++ = 255 - v; // blue
341  *palette_ptr++ = 255; // alpha
342  }
343  // inscribed obstacle values (99) in cyan
344  *palette_ptr++ = 0; // red
345  *palette_ptr++ = 255; // green
346  *palette_ptr++ = 255; // blue
347  *palette_ptr++ = 255; // alpha
348  // lethal obstacle values (100) in purple
349  *palette_ptr++ = 255; // red
350  *palette_ptr++ = 0; // green
351  *palette_ptr++ = 255; // blue
352  *palette_ptr++ = 255; // alpha
353  // illegal positive values in green
354  for (int i = 101; i <= 127; i++)
355  {
356  *palette_ptr++ = 0; // red
357  *palette_ptr++ = 255; // green
358  *palette_ptr++ = 0; // blue
359  *palette_ptr++ = 255; // alpha
360  }
361  // illegal negative (char) values in shades of red/yellow
362  for (int i = 128; i <= 254; i++)
363  {
364  *palette_ptr++ = 255; // red
365  *palette_ptr++ = (255 * (i - 128)) / (254 - 128); // green
366  *palette_ptr++ = 0; // blue
367  *palette_ptr++ = 255; // alpha
368  }
369  // legal -1 value is tasteful blueish greenish grayish color
370  *palette_ptr++ = 0x70; // red
371  *palette_ptr++ = 0x89; // green
372  *palette_ptr++ = 0x86; // blue
373  *palette_ptr++ = 255; // alpha
374 
375  return palette;
376 }
377 
378 unsigned char* makeRawPalette()
379 {
380  unsigned char* palette = OGRE_ALLOC_T(unsigned char, 256 * 4, Ogre::MEMCATEGORY_GENERAL);
381  unsigned char* palette_ptr = palette;
382  // Standard gray map palette values
383  for (int i = 0; i < 256; i++)
384  {
385  *palette_ptr++ = i; // red
386  *palette_ptr++ = i; // green
387  *palette_ptr++ = i; // blue
388  *palette_ptr++ = 255; // alpha
389  }
390 
391  return palette;
392 }
393 
394 Ogre::TexturePtr makePaletteTexture(unsigned char* palette_bytes)
395 {
396  Ogre::DataStreamPtr palette_stream;
397  palette_stream.bind(new Ogre::MemoryDataStream(palette_bytes, 256 * 4, true));
398 
399  static int palette_tex_count = 0;
400  std::stringstream ss;
401  ss << "MapPaletteTexture" << palette_tex_count++;
402  return Ogre::TextureManager::getSingleton().loadRawData(
403  ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, palette_stream, 256, 1,
404  Ogre::PF_BYTE_RGBA, Ogre::TEX_TYPE_1D, 0);
405 }
406 
408 {
409  // Order of palette textures here must match option indices for color_scheme_property_ above.
411  color_scheme_transparency_.push_back(false);
413  color_scheme_transparency_.push_back(true);
415  color_scheme_transparency_.push_back(true);
416 }
417 
419 {
420  subscribe();
421 }
422 
424 {
425  unsubscribe();
426  clear();
427 }
428 
430 {
431  if (!isEnabled())
432  {
433  return;
434  }
435 
436  if (!topic_property_->getTopic().isEmpty())
437  {
438  try
439  {
441  {
443  this, ros::TransportHints().unreliable());
444  }
445  else
446  {
448  this, ros::TransportHints().reliable());
449  }
450  setStatus(StatusProperty::Ok, "Topic", "OK");
451  }
452  catch (ros::Exception& e)
453  {
454  setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
455  }
456 
457  try
458  {
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  {
497  swatches[i]->updateAlpha(sceneBlending, depthWrite, &alpha_setter);
498  }
499 }
500 
502 {
503  bool draw_under = draw_under_property_->getValue().toBool();
504 
505  if (alpha_property_->getFloat() >= 0.9998)
506  {
507  for (unsigned i = 0; i < swatches.size(); i++)
508  swatches[i]->material_->setDepthWriteEnabled(!draw_under);
509  }
510 
511  int group = draw_under ? Ogre::RENDER_QUEUE_4 : Ogre::RENDER_QUEUE_MAIN;
512  for (unsigned i = 0; i < swatches.size(); i++)
513  {
514  if (swatches[i]->manual_object_)
515  swatches[i]->manual_object_->setRenderQueueGroup(group);
516  }
517 }
518 
520 {
521  unsubscribe();
522  subscribe();
523  clear();
524 }
525 
527 {
528  setStatus(StatusProperty::Warn, "Message", "No map received");
529 
530  if (!loaded_)
531  {
532  return;
533  }
534 
535  for (unsigned i = 0; i < swatches.size(); i++)
536  {
537  if (swatches[i]->manual_object_)
538  swatches[i]->manual_object_->setVisible(false);
539 
540  if (!swatches[i]->texture_.isNull())
541  {
542  Ogre::TextureManager::getSingleton().remove(swatches[i]->texture_->getName());
543  swatches[i]->texture_.setNull();
544  }
545  }
546 
547  loaded_ = false;
548  map_updated_ = false;
549 }
550 
551 bool validateFloats(const nav_msgs::OccupancyGrid& msg)
552 {
553  bool valid = true;
554  valid = valid && validateFloats(msg.info.resolution);
555  valid = valid && validateFloats(msg.info.origin);
556  return valid;
557 }
558 
559 void MapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
560 {
561  current_map_ = *msg;
562  loaded_ = true;
563  map_updated_ = true;
564 }
565 
566 
567 void MapDisplay::incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& update)
568 {
569  // Only update the map if we have gotten a full one first.
570  if (!loaded_)
571  {
572  return;
573  }
574 
575  // Reject updates which have any out-of-bounds data.
576  if (update->x < 0 || update->y < 0 || current_map_.info.width < update->x + update->width ||
577  current_map_.info.height < update->y + update->height)
578  {
579  setStatus(StatusProperty::Error, "Update", "Update area outside of original map area.");
580  return;
581  }
582 
583  // Copy the incoming data into current_map_'s data.
584  for (size_t y = 0; y < update->height; y++)
585  {
586  memcpy(&current_map_.data[(update->y + y) * current_map_.info.width + update->x],
587  &update->data[y * update->width], update->width);
588  }
589  map_updated_ = true;
590 }
591 
593 {
594  int width = current_map_.info.width;
595  int height = current_map_.info.height;
596  float resolution = current_map_.info.resolution;
597 
598  int sw = width;
599  int sh = height;
600  int n_swatches = 1;
601 
602  for (int i = 0; i < 4; i++)
603  {
604  ROS_INFO("Creating %d swatches", n_swatches);
605  for (unsigned i = 0; i < swatches.size(); i++)
606  {
607  delete swatches[i];
608  }
609  swatches.clear();
610  try
611  {
612  int x = 0;
613  int y = 0;
614  for (int i = 0; i < n_swatches; i++)
615  {
616  int tw, th;
617  if (width - x - sw >= sw)
618  tw = sw;
619  else
620  tw = width - x;
621 
622  if (height - y - sh >= sh)
623  th = sh;
624  else
625  th = height - y;
626 
627  swatches.push_back(new Swatch(this, x, y, tw, th, resolution));
628  swatches[i]->updateData();
629 
630  x += tw;
631  if (x >= width)
632  {
633  x = 0;
634  y += sh;
635  }
636  }
637  updateAlpha();
638  return;
639  }
640  catch (Ogre::RenderingAPIException&)
641  {
642  ROS_WARN("Failed to create %d swatches", n_swatches);
643  if (sw > sh)
644  sw /= 2;
645  else
646  sh /= 2;
647  n_swatches *= 2;
648  }
649  }
650 }
651 
653 {
654  if (current_map_.data.empty())
655  {
656  return;
657  }
658 
660  {
662  "Message contained invalid floating point values (nans or infs)");
663  return;
664  }
665 
666  if (!validateQuaternions(current_map_.info.origin))
667  {
668  ROS_WARN_ONCE_NAMED("quaternions",
669  "Map received on topic '%s' contains unnormalized quaternions. "
670  "This warning will only be output once but may be true for others; "
671  "enable DEBUG messages for ros.rviz.quaternions to see more details.",
672  topic_property_->getTopicStd().c_str());
673  ROS_DEBUG_NAMED("quaternions", "Map received on topic '%s' contains unnormalized quaternions.",
674  topic_property_->getTopicStd().c_str());
675  }
676 
677  if (current_map_.info.width * current_map_.info.height == 0)
678  {
679  std::stringstream ss;
680  ss << "Map is zero-sized (" << current_map_.info.width << "x" << current_map_.info.height << ")";
681  setStatus(StatusProperty::Error, "Map", QString::fromStdString(ss.str()));
682  return;
683  }
684 
685  setStatus(StatusProperty::Ok, "Message", "Map received");
686 
687  ROS_DEBUG("Received a %d X %d map @ %.3f m/pix\n", current_map_.info.width, current_map_.info.height,
688  current_map_.info.resolution);
689 
690  float resolution = current_map_.info.resolution;
691 
692  int width = current_map_.info.width;
693  int height = current_map_.info.height;
694 
695  if (width != width_ || height != height_ || resolution_ != resolution)
696  {
697  createSwatches();
698  width_ = width;
699  height_ = height;
700  resolution_ = resolution;
701  }
702 
703  Ogre::Vector3 position(current_map_.info.origin.position.x, current_map_.info.origin.position.y,
704  current_map_.info.origin.position.z);
705  Ogre::Quaternion orientation;
706  normalizeQuaternion(current_map_.info.origin.orientation, orientation);
707 
708  frame_ = current_map_.header.frame_id;
709  if (frame_.empty())
710  {
711  frame_ = "map";
712  }
713 
714  bool map_status_set = false;
715  if (width * height != static_cast<int>(current_map_.data.size()))
716  {
717  std::stringstream ss;
718  ss << "Data size doesn't match width*height: width = " << width << ", height = " << height
719  << ", data size = " << current_map_.data.size();
720  setStatus(StatusProperty::Error, "Map", QString::fromStdString(ss.str()));
721  map_status_set = true;
722  }
723 
724  for (size_t i = 0; i < swatches.size(); i++)
725  {
726  swatches[i]->updateData();
727 
728  Ogre::Pass* pass = swatches[i]->material_->getTechnique(0)->getPass(0);
729  Ogre::TextureUnitState* tex_unit = nullptr;
730  if (pass->getNumTextureUnitStates() > 0)
731  {
732  tex_unit = pass->getTextureUnitState(0);
733  }
734  else
735  {
736  tex_unit = pass->createTextureUnitState();
737  }
738 
739  tex_unit->setTextureName(swatches[i]->texture_->getName());
740  tex_unit->setTextureFiltering(Ogre::TFO_NONE);
741  swatches[i]->manual_object_->setVisible(true);
742  }
743 
744 
745  if (!map_status_set)
746  {
747  setStatus(StatusProperty::Ok, "Map", "Map OK");
748  }
749  updatePalette();
750 
751  resolution_property_->setValue(resolution);
752  width_property_->setValue(width);
753  height_property_->setValue(height);
754  position_property_->setVector(position);
755  orientation_property_->setQuaternion(orientation);
756 
757  map_updated_ = false;
758 }
759 
761 {
762  int palette_index = color_scheme_property_->getOptionInt();
763 
764  for (unsigned i = 0; i < swatches.size(); i++)
765  {
766  Ogre::Pass* pass = swatches[i]->material_->getTechnique(0)->getPass(0);
767  Ogre::TextureUnitState* palette_tex_unit = nullptr;
768  if (pass->getNumTextureUnitStates() > 1)
769  {
770  palette_tex_unit = pass->getTextureUnitState(1);
771  }
772  else
773  {
774  palette_tex_unit = pass->createTextureUnitState();
775  }
776  palette_tex_unit->setTextureName(palette_textures_[palette_index]->getName());
777  palette_tex_unit->setTextureFiltering(Ogre::TFO_NONE);
778  }
779 
780  updateAlpha();
781 }
782 
784 {
785  if (!loaded_)
786  {
787  return;
788  }
789 
790  ros::Time transform_time;
791 
793  {
794  transform_time = current_map_.header.stamp;
795  }
796 
797  Ogre::Vector3 position;
798  Ogre::Quaternion orientation;
799  if (!context_->getFrameManager()->transform(frame_, transform_time, current_map_.info.origin, position,
800  orientation) &&
801  !context_->getFrameManager()->transform(frame_, ros::Time(0), current_map_.info.origin, position,
802  orientation))
803  {
804  ROS_DEBUG("Error transforming map '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
805  frame_.c_str(), qPrintable(fixed_frame_));
806 
807  setStatus(StatusProperty::Error, "Transform",
808  "No transform from [" + QString::fromStdString(frame_) + "] to [" + fixed_frame_ + "]");
809  }
810  else
811  {
812  setStatus(StatusProperty::Ok, "Transform", "Transform OK");
813  }
814 
815  scene_node_->setPosition(position);
816  scene_node_->setOrientation(orientation);
817 }
818 
820 {
821  transformMap();
822 }
823 
825 {
826  Display::reset();
827 
828  clear();
829  // Force resubscription so that the map will be re-sent
830  updateTopic();
831 }
832 
833 void MapDisplay::setTopic(const QString& topic, const QString& /*datatype*/)
834 {
835  topic_property_->setString(topic);
836 }
837 
838 void MapDisplay::update(float /*wall_dt*/, float /*ros_dt*/)
839 {
840  if (map_updated_)
841  updateMap();
842  transformMap();
843 }
844 
845 } // namespace rviz
846 
rviz::MapDisplay::swatches
std::vector< Swatch * > swatches
Definition: map_display.h:152
ros::TransportHints::unreliable
TransportHints & unreliable()
rviz::BoolProperty::getBool
virtual bool getBool() const
Definition: bool_property.cpp:48
rviz::EnumProperty::getOptionInt
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
Definition: enum_property.cpp:56
rviz::Display::isEnabled
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:271
rviz::MapDisplay::updateAlpha
void updateAlpha()
Definition: map_display.cpp:476
rviz::AlphaSetter::alpha_vec_
Ogre::Vector4 alpha_vec_
Definition: map_display.cpp:77
rviz::RosTopicProperty
Definition: ros_topic_property.h:39
validate_floats.h
rviz::Swatch::parent_
MapDisplay * parent_
Definition: map_display.h:84
rviz::Swatch::updateData
void updateData()
Definition: map_display.cpp:183
rviz::MapDisplay::updatePalette
void updatePalette()
Definition: map_display.cpp:760
frame_manager.h
rviz::MapDisplay::Swatch
friend class Swatch
Definition: map_display.h:99
rviz::StatusProperty::Error
@ Error
Definition: status_property.h:46
rviz::MapDisplay::topic_property_
RosTopicProperty * topic_property_
Definition: map_display.h:168
ros_topic_property.h
rviz::Swatch::manual_object_
Ogre::ManualObject * manual_object_
Definition: map_display.h:85
rviz::MapDisplay::height_property_
IntProperty * height_property_
Definition: map_display.h:171
rviz::MapDisplay::color_scheme_property_
EnumProperty * color_scheme_property_
Definition: map_display.h:176
rviz::FloatProperty::setValue
bool setValue(const QVariant &new_value) override
Set the new value for this property. Returns true if the new value is different from the old value,...
Definition: float_property.cpp:46
ros.h
property.h
rviz::MapDisplay::updateTopic
void updateTopic()
Definition: map_display.cpp:519
custom_parameter_indices.h
int_property.h
rviz::FloatProperty::setMax
void setMax(float max)
Definition: float_property.cpp:57
rviz::EditableEnumProperty::setString
virtual void setString(const QString &str)
Definition: editable_enum_property.cpp:74
rviz::Swatch::Swatch
Swatch(MapDisplay *parent, unsigned int x, unsigned int y, unsigned int width, unsigned int height, float resolution)
Definition: map_display.cpp:81
rviz::MapDisplay::createSwatches
void createSwatches()
Definition: map_display.cpp:592
ros::TransportHints
rviz::validateFloats
bool validateFloats(const sensor_msgs::CameraInfo &msg)
Definition: camera_display.cpp:72
rviz::MapDisplay::map_sub_
ros::Subscriber map_sub_
Definition: map_display.h:165
grid.h
rviz::MapDisplay::transform_timestamp_property_
BoolProperty * transform_timestamp_property_
Definition: map_display.h:179
rviz::MapDisplay::resolution_property_
FloatProperty * resolution_property_
Definition: map_display.h:169
rviz::MapDisplay::orientation_property_
QuaternionProperty * orientation_property_
Definition: map_display.h:173
ros::Subscriber::shutdown
void shutdown()
float_property.h
ROS_WARN_ONCE_NAMED
#define ROS_WARN_ONCE_NAMED(name,...)
ros::Exception
rviz::Display::fixed_frame_
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
rviz::MapDisplay::update_sub_
ros::Subscriber update_sub_
Definition: map_display.h:166
validate_quaternions.h
rviz::QuaternionProperty::setReadOnly
void setReadOnly(bool read_only) override
Overridden from Property to propagate read-only-ness to children.
Definition: quaternion_property.cpp:158
rviz::AlphaSetter::visit
void visit(Ogre::Renderable *rend, ushort, bool, Ogre::Any *=nullptr) override
Definition: map_display.cpp:68
rviz::makeCostmapPalette
unsigned char * makeCostmapPalette()
Definition: map_display.cpp:323
rviz::Property::getValue
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:150
rviz::MapDisplay::color_scheme_transparency_
std::vector< bool > color_scheme_transparency_
Definition: map_display.h:154
rviz::MapDisplay::width_
int width_
Definition: map_display.h:160
rviz::makeMapPalette
unsigned char * makeMapPalette()
Definition: map_display.cpp:285
rviz::Property::Property
Property(const QString &name=QString(), const QVariant &default_value=QVariant(), const QString &description=QString(), Property *parent=nullptr)
Constructor.
Definition: property.cpp:59
rviz::BoolProperty::BoolProperty
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=nullptr)
Definition: bool_property.cpp:36
rviz::FloatProperty::setMin
void setMin(float min)
Definition: float_property.cpp:51
rviz::MapDisplay::updateMap
void updateMap()
Definition: map_display.cpp:652
f
f
rviz::MapDisplay::alpha_property_
FloatProperty * alpha_property_
Definition: map_display.h:174
quaternion_property.h
rviz::Display
Definition: display.h:63
rviz::EnumProperty
Enum property.
Definition: enum_property.h:46
rviz::QuaternionProperty
Definition: quaternion_property.h:38
rviz::FloatProperty
Property specialized to enforce floating point max/min.
Definition: float_property.h:37
rviz::Swatch::height_
unsigned int height_
Definition: map_display.h:89
rviz::IntProperty::setValue
bool setValue(const QVariant &new_value) override
Set the new value for this property. Returns true if the new value is different from the old value,...
Definition: int_property.cpp:47
rviz::MapDisplay::onDisable
void onDisable() override
Derived classes override this to do the actual work of disabling themselves.
Definition: map_display.cpp:423
rviz::Swatch::x_
unsigned int x_
Definition: map_display.h:89
ros::TransportHints::reliable
TransportHints & reliable()
rviz::Display::setStatus
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:176
rviz::normalizeQuaternion
float normalizeQuaternion(float &w, float &x, float &y, float &z)
Definition: validate_quaternions.h:66
rviz::Swatch::updateAlpha
void updateAlpha(const Ogre::SceneBlendType sceneBlending, bool depthWrite, AlphaSetter *alpha_setter)
Definition: map_display.cpp:171
rviz::MapDisplay::transformMap
void transformMap()
Definition: map_display.cpp:783
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
rviz::FloatProperty::getFloat
virtual float getFloat() const
Definition: float_property.h:79
rviz::EnumProperty::addOption
virtual void addOption(const QString &option, int value=0)
Definition: enum_property.cpp:50
rviz::MapDisplay::~MapDisplay
~MapDisplay() override
Definition: map_display.cpp:274
rviz::Swatch::scene_node_
Ogre::SceneNode * scene_node_
Definition: map_display.h:88
ROS_DEBUG
#define ROS_DEBUG(...)
rviz::Swatch::texture_
Ogre::TexturePtr texture_
Definition: map_display.h:86
rviz::MapDisplay::current_map_
nav_msgs::OccupancyGrid current_map_
Definition: map_display.h:163
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
rviz
Definition: add_display_dialog.cpp:54
rviz::Display::scene_node_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
rviz::VectorProperty::setVector
virtual bool setVector(const Ogre::Vector3 &vector)
Definition: vector_property.cpp:55
rviz::QuaternionProperty::setQuaternion
virtual bool setQuaternion(const Ogre::Quaternion &quaternion)
Definition: quaternion_property.cpp:60
rviz::MapDisplay::loaded_
bool loaded_
Definition: map_display.h:155
rviz::RosTopicProperty::getTopic
QString getTopic() const
Definition: ros_topic_property.h:81
ALPHA_PARAMETER
#define ALPHA_PARAMETER
Definition: custom_parameter_indices.h:42
rviz::MapDisplay::fixedFrameChanged
void fixedFrameChanged() override
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
Definition: map_display.cpp:819
rviz::StatusProperty::Ok
@ Ok
Definition: status_property.h:44
rviz::MapDisplay::incomingMap
void incomingMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
Copy msg into current_map_ and set flag map_updated_.
Definition: map_display.cpp:559
rviz::MapDisplay::resolution_
float resolution_
Definition: map_display.h:159
rviz::validateQuaternions
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
Definition: interactive_marker_display.cpp:63
rviz::StatusProperty::Warn
@ Warn
Definition: status_property.h:45
rviz::Swatch::y_
unsigned int y_
Definition: map_display.h:89
rviz::Display::scene_manager_
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Definition: display.h:292
ROS_WARN
#define ROS_WARN(...)
rviz::Swatch::material_
Ogre::MaterialPtr material_
Definition: map_display.h:87
rviz::MapDisplay::update
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
Definition: map_display.cpp:838
rviz::MapDisplay::unsubscribe
virtual void unsubscribe()
Definition: map_display.cpp:470
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
rviz::MapDisplay::unreliable_property_
BoolProperty * unreliable_property_
Definition: map_display.h:178
rviz::makeRawPalette
unsigned char * makeRawPalette()
Definition: map_display.cpp:378
rviz::DisplayContext::getFrameManager
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
rviz::Swatch::width_
unsigned int width_
Definition: map_display.h:89
rviz::MapDisplay::draw_under_property_
Property * draw_under_property_
Definition: map_display.h:175
rviz::RosTopicProperty::getTopicStd
std::string getTopicStd() const
Definition: ros_topic_property.h:85
rviz::AlphaSetter::AlphaSetter
AlphaSetter(float alpha)
Definition: map_display.cpp:64
map_display.h
rviz::MapDisplay::palette_textures_
std::vector< Ogre::TexturePtr > palette_textures_
Definition: map_display.h:153
rviz::MapDisplay::height_
int height_
Definition: map_display.h:161
rviz::Property::getName
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:164
rviz::Display::context_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz....
Definition: display.h:287
rviz::MapDisplay::subscribe
virtual void subscribe()
Definition: map_display.cpp:429
ros::Time
rviz::Property::setReadOnly
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:497
vector_property.h
rviz::VectorProperty::setReadOnly
void setReadOnly(bool read_only) override
Overridden from Property to propagate read-only-ness to children.
Definition: vector_property.cpp:143
rviz::MapDisplay::width_property_
IntProperty * width_property_
Definition: map_display.h:170
class_list_macros.hpp
rviz::FrameManager::transform
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.
Definition: frame_manager.h:148
rviz::MapDisplay::updateDrawUnder
void updateDrawUnder()
Definition: map_display.cpp:501
rviz::MapDisplay::incomingUpdate
void incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr &update)
Copy update's data into current_map_ and set flag map_updated_.
Definition: map_display.cpp:567
rviz::Display::reset
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:290
rviz::MapDisplay::onEnable
void onEnable() override
Derived classes override this to do the actual work of enabling themselves.
Definition: map_display.cpp:418
rviz::makePaletteTexture
Ogre::TexturePtr makePaletteTexture(unsigned char *palette_bytes)
Definition: map_display.cpp:394
display_context.h
rviz::VectorProperty
Definition: vector_property.h:39
rviz::MapDisplay
Displays a map along the XY plane.
Definition: map_display.h:97
rviz::MapDisplay::setTopic
void setTopic(const QString &topic, const QString &datatype) override
Set the ROS topic to listen to for this display.
Definition: map_display.cpp:833
ROS_INFO
#define ROS_INFO(...)
rviz::MapDisplay::position_property_
VectorProperty * position_property_
Definition: map_display.h:172
rviz::MapDisplay::onInitialize
void onInitialize() override
Override this function to do subclass-specific initialization.
Definition: map_display.cpp:407
rviz::AlphaSetter
Definition: map_display.cpp:61
rviz::MapDisplay::reset
void reset() override
Called to tell the display to clear its state.
Definition: map_display.cpp:824
rviz::MapDisplay::frame_
std::string frame_
Definition: map_display.h:162
rviz::Display::update_nh_
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
enum_property.h
rviz::IntProperty
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:37
rviz::MapDisplay::MapDisplay
MapDisplay()
Definition: map_display.cpp:222
rviz::MapDisplay::clear
void clear()
Definition: map_display.cpp:526
rviz::MapDisplay::map_updated_
bool map_updated_
Definition: map_display.h:156
rviz::Swatch::~Swatch
~Swatch()
Definition: map_display.cpp:166


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:09