Fix map display (#425) (#432)

* Improve test for large maps

The test fails for the current implementation as the map is too big.
This should not be the case though.

* Fix swatches to store more data

The API of loadRawData takes an Ogre ushort which is
an unsigned short which is guaranteed to be 16 bit.
Hence we should static_cast to a uint16_t instead of a uint8_t.

This will result in the swatch being creatable for much larger
textures

* Minor refactoring of map display

- Explain rationale behind maximum swatch split
- Improve const correctness
- Log better error if the map cannot be displayed
- Move method updateDrawnUnder() to place where it
  can actually be executed

* Improve log messages

rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp

@@ -328,13 +328,18 @@ void MapDisplay::createSwatches()
   size_t swatch_width = width;
   size_t swatch_height = height;
   int number_swatches = 1;
-  int maximum_number_swatch_splittings = 4;  // 4 seems to work well for this purpose.
-
-  for (int i = 0; i < maximum_number_swatch_splittings; i++) {
-    RVIZ_COMMON_LOG_INFO_STREAM("Creating " << number_swatches << " swatches_");
+  // One swatch can have up to 2^16 * 2^16 pixel (8 bit texture, i.e. 4GB of data)
+  // Since the width and height are separately limited by 2^16 it might be necessary to have several
+  // pieces, however more than 8 swatches is probably unnecessary due to memory limitations
+  const size_t maximum_number_swatch_splittings = 4;
+
+  for (size_t i = 0; i < maximum_number_swatch_splittings; ++i) {
+    RVIZ_COMMON_LOG_INFO_STREAM("Trying to create a map of size " <<
+      width << " x " << height << " using " << number_swatches << " swatches");
     swatches_.clear();
     try {
       tryCreateSwatches(width, height, resolution, swatch_width, swatch_height, number_swatches);
+      updateDrawUnder();
       return;
     } catch (Ogre::InvalidParametersException &) {
       doubleSwatchNumber(swatch_width, swatch_height, number_swatches);
@@ -343,13 +348,16 @@ void MapDisplay::createSwatches()
       doubleSwatchNumber(swatch_width, swatch_height, number_swatches);
     }
   }
-  updateDrawUnder();
+  RVIZ_COMMON_LOG_ERROR_STREAM("Creating " << number_swatches << "failed. "
+    "This map is too large to be displayed by RViz.");
+  swatches_.clear();
 }
 
 void MapDisplay::doubleSwatchNumber(
   size_t & swatch_width, size_t & swatch_height, int & number_swatches) const
 {
-  RVIZ_COMMON_LOG_ERROR_STREAM("Failed to create " << number_swatches << " swatches_");
+  RVIZ_COMMON_LOG_ERROR_STREAM("Failed to create map using " << number_swatches << " swatches. "
+    "At least one swatch seems to need too much memory");
   if (swatch_width > swatch_height) {
     swatch_width /= 2;
   } else {

rviz_default_plugins/src/rviz_default_plugins/displays/map/swatch.cpp

@@ -209,7 +209,7 @@ void Swatch::resetTexture(Ogre::DataStreamPtr & pixel_stream)
     "MapTexture" + std::to_string(texture_count_++),
     "rviz_rendering",
     pixel_stream,
-    static_cast<uint8_t>(width_), static_cast<uint8_t>(height_),
+    static_cast<uint16_t>(width_), static_cast<uint16_t>(height_),
     Ogre::PF_L8, Ogre::TEX_TYPE_2D, 0);
 }
 

rviz_default_plugins/test/rviz_default_plugins/displays/map/map_display_test.cpp

@@ -204,10 +204,11 @@ TEST_F(MapTestFixture, reset_deletes_map) {
 }
 
 TEST_F(MapTestFixture, createSwatches_creates_more_swatches_if_map_is_too_big) {
-  map_display_->processMessage(createMapMessage(500, 500));
+  // one dimension is larger than 2^16 --> that's too much for one texture buffer
+  map_display_->processMessage(createMapMessage(70000, 50));
 
   auto manual_objects = rviz_rendering::findAllOgreObjectByType<Ogre::ManualObject>(
     scene_manager_->getRootSceneNode(), "ManualObject");
 
-  EXPECT_THAT(manual_objects, SizeIs(4));
+  EXPECT_THAT(manual_objects, SizeIs(2));
 }