Rewrite tile_map loading to be more reliable

This changes how the tile_map plugin handles making network requires for tiles.
It will now:
- Use thread conditions to prompt loading rather than spinning
- Prioritize loading tiles in the visible area
- Retry a failed network request up to 5 times
- Not discard tile requests if there are more than 100 in the queue

This changes should significantly reduce (if not completely eliminate) the
number of tiles that fail to load and hopefully make tiles within the visible
area appear faster when there are many in the queue.

Fixes swri-robotics#342 and swri-robotics#421.

tile_map/include/tile_map/image_cache.h

@@ -31,6 +31,7 @@
 #define TILE_MAP_IMAGE_CACHE_H_
 
 #include <string>
+#include <limits>
 
 #include <boost/cstdint.hpp>
 #include <boost/shared_ptr.hpp>
@@ -41,7 +42,9 @@
 #include <QMutex>
 #include <QNetworkReply>
 #include <QObject>
+#include <QSemaphore>
 #include <QThread>
+#include <set>
 
 namespace tile_map
 {
@@ -64,6 +67,13 @@ namespace tile_map
     void AddFailure();
     bool Failed() const { return failed_; }
 
+    void IncreasePriority()
+    {
+      if (priority_ < std::numeric_limits<uint64_t>::max())
+      {
+        priority_++;
+      }
+    }
     void SetPriority(uint64_t priority) { priority_ = priority; }
     uint64_t Priority() const { return priority_; }
 
@@ -81,6 +91,8 @@ namespace tile_map
     uint64_t priority_;
 
     mutable boost::shared_ptr<QImage> image_;
+
+    static const int MAXIMUM_FAILURES;
   };
   typedef boost::shared_ptr<Image> ImagePtr;
 
@@ -112,28 +124,36 @@ namespace tile_map
     QMutex cache_mutex_;
     QMutex unprocessed_mutex_;
     bool exit_;
-
-    int32_t pending_;
+
     uint64_t tick_;
 
     CacheThread* cache_thread_;
-
+
+    QSemaphore network_request_semaphore_;
+
     friend class CacheThread;
+
+    static const int MAXIMUM_NETWORK_REQUESTS;
   };
 
   class CacheThread : public QThread
   {
     Q_OBJECT
     public:
-      explicit CacheThread(ImageCache* parent) : p(parent) {}
+      explicit CacheThread(ImageCache* parent);
 
       virtual void run();
 
+      void notify();
+
     Q_SIGNALS:
       void RequestImage(QString);
 
     private:
       ImageCache* p;
+      QMutex waiting_mutex_;
+
+      static const int MAXIMUM_SEQUENTIAL_REQUESTS;
   };
 
 

tile_map/src/image_cache.cpp

@@ -47,6 +47,8 @@ namespace tile_map
     return left->Priority() > right->Priority();
   }
 
+  const int Image::MAXIMUM_FAILURES = 5;
+
   Image::Image(const QString& uri, size_t uri_hash, uint64_t priority) :
     uri_(uri),
     uri_hash_(uri_hash),
@@ -74,17 +76,19 @@ namespace tile_map
   void Image::AddFailure()
   {
     failures_++;
-    failed_ = failures_ > 2;
+    failed_ = failures_ > MAXIMUM_FAILURES;
   }
 
+  const int ImageCache::MAXIMUM_NETWORK_REQUESTS = 6;
+
   ImageCache::ImageCache(const QString& cache_dir, size_t size) :
     network_manager_(this),
     cache_dir_(cache_dir),
     cache_(size),
     exit_(false),
-    pending_(0),
     tick_(0),
-    cache_thread_(new CacheThread(this))
+    cache_thread_(new CacheThread(this)),
+    network_request_semaphore_(MAXIMUM_NETWORK_REQUESTS)
   {
     QNetworkDiskCache* disk_cache = new QNetworkDiskCache(this);
     disk_cache->setCacheDirectory(cache_dir_);
@@ -99,7 +103,11 @@ namespace tile_map
 
   ImageCache::~ImageCache()
   {
+    // After setting our exit flag to true, release any conditions the cache thread
+    // might be waiting on so that it will exit.
     exit_ = true;
+    cache_thread_->notify();
+    network_request_semaphore_.release(MAXIMUM_NETWORK_REQUESTS);
     cache_thread_->wait();
     delete cache_thread_;
   }
@@ -145,9 +153,22 @@ namespace tile_map
       {
         if (!unprocessed_.contains(uri_hash))
         {
+          // Set an image's starting priority so that it's higher than the
+          // starting priority of every other image we've requested so
+          // far; that ensures that, all other things being equal, the
+          // most recently requested images will be loaded first.
           image->SetPriority(tick_++);
           unprocessed_[uri_hash] = image;
           uri_to_hash_map_[uri] = uri_hash;
+          cache_thread_->notify();
+        }
+        else
+        {
+          // Every time an image is requested but hasn't been loaded yet,
+          // increase its priority.  Tiles within the visible area will
+          // be requested more frequently, so this will make them load faster
+          // than tiles the user can't see.
+          image->IncreasePriority();
         }
       }
       else
@@ -172,7 +193,8 @@ namespace tile_map
     request.setAttribute(
         QNetworkRequest::HttpPipeliningAllowedAttribute,
         true);
-
+
+    ROS_INFO("Trying to get %s", uri.toStdString().c_str());
     QNetworkReply *reply = network_manager_.get(request);
     connect(reply, SIGNAL(error(QNetworkReply::NetworkError)),
             this, SLOT(NetworkError(QNetworkReply::NetworkError)));
@@ -213,39 +235,59 @@ namespace tile_map
     {
       image->SetLoading(false);
     }
-
-    pending_--;
-
+    network_request_semaphore_.release();
+
     unprocessed_mutex_.unlock();
 
     reply->deleteLater();
-
-    // Condition variable?
   }
 
   void ImageCache::NetworkError(QNetworkReply::NetworkError error)
   {
     ROS_ERROR("NETWORK ERROR");
     // TODO add failure
   }
+
+  const int CacheThread::MAXIMUM_SEQUENTIAL_REQUESTS = 12;
+
+  CacheThread::CacheThread(ImageCache* parent) :
+    p(parent),
+    waiting_mutex_()
+  {
+    waiting_mutex_.lock();
+  }
+
+  void CacheThread::notify()
+  {
+    waiting_mutex_.unlock();
+  }
 
   void CacheThread::run()
   {    
     while (!p->exit_)
     {
-      // TODO: Condition variable to wait for pending < 6 ?
-
+      // Wait until we're told there are images we need to request.
+      waiting_mutex_.lock();
+
+      // Next, get all of them and sort them by priority.
       p->unprocessed_mutex_.lock();
-
       QList<ImagePtr> images = p->unprocessed_.values();
-
       p->unprocessed_mutex_.unlock();
-      
+
       qSort(images.begin(), images.end(), ComparePriority);
-
-      int32_t count = 0;
-      while (p->pending_ < 6 && !images.empty() && count < 12)
+
+      // Go through all of them and request them.  Qt's network manager will
+      // only handle six simultaneous requests at once, so we use a semaphore
+      // to limit ourselves to that many.
+      // Each individual image will release the semaphore when it is done loading.
+      // Also, only load up to a certain number at a time in this loop.  If there
+      // are more left afterward, we'll start over.  This ensures that we
+      // concentrate on processing the highest-priority images.
+      int count = 0;
+      while (!p->exit_ && !images.empty() && count < MAXIMUM_SEQUENTIAL_REQUESTS)
       {
+        p->network_request_semaphore_.acquire();
+
         ImagePtr image = images.front();
         p->unprocessed_mutex_.lock();
         if (!image->Loading())
@@ -254,29 +296,19 @@ namespace tile_map
           images.pop_front();
 
           Q_EMIT RequestImage(image->Uri());
-
-          p->pending_++;
-          count++;
         }
         else
         {
           images.pop_front();
         }
         p->unprocessed_mutex_.unlock();
+
+        count++;
       }
-
-      p->unprocessed_mutex_.lock();
-      // Remove the oldest images from the unprocessed list.
-      while (images.size() > 100)
+      if (!images.empty())
       {
-        ImagePtr image = images.back();
-        images.pop_back();
-        p->unprocessed_.remove(image->UriHash());
-        p->uri_to_hash_map_.remove(image->Uri());
+        waiting_mutex_.unlock();
       }
-      p->unprocessed_mutex_.unlock();
-
-      usleep(1000);
     }
   }
 }