fixed quantity query of historical surface

robotology · Dec 6, 2017 · 240aa21 · 240aa21
1 parent 1047ab7
commit 240aa21
Show file tree

Hide file tree

Showing 3 changed files with 88 additions and 1 deletion.
diff --git a/libraries/include/iCub/eventdriven/vSurfaceHandlerTh.h b/libraries/include/iCub/eventdriven/vSurfaceHandlerTh.h
@@ -398,8 +398,50 @@ class hSurfThread : public yarp::os::Thread
 
     }
 
+    vQueue queryROI(int channel, int numEvts, int r)
+    {
+
+        vQueue q;
+
+        m.lock();
+        double cpunow = yarp::os::Time::now();
+
+        if(channel == 0) {
+
+            cpudelayL -= (cpunow - cputimeL) * vtsHelper::vtsscaler * 1.1;
+            cputimeL = cpunow;
+
+            if(cpudelayL < 0) cpudelayL = 0;
+            if(cpudelayL > maxcpudelay) {
+                yWarning() << "CPU delay hit maximum";
+                cpudelayL = maxcpudelay;
+            }
+
+            surfaceleft.getSurfaceN(q, cpudelayL, numEvts, r);
+        }
+        else {
+
+            cpudelayR -= (cpunow - cputimeR) * vtsHelper::vtsscaler * 1.1;
+            cputimeR = cpunow;
+
+            if(cpudelayR < 0) cpudelayR = 0;
+            if(cpudelayR > maxcpudelay) {
+                yWarning() << "CPU delay hit maximum";
+                cpudelayR = maxcpudelay;
+            }
+
+            surfaceright.getSurfaceN(q, cpudelayR, numEvts, r);
+        }
+
+        m.unlock();
+
+        return q;
+    }
+
     vQueue queryROI(int channel, unsigned int querySize, int x, int y, int r)
     {
+
+
         vQueue q;
 
         m.lock();

diff --git a/libraries/include/iCub/eventdriven/vWindow_adv.h b/libraries/include/iCub/eventdriven/vWindow_adv.h
@@ -239,7 +239,9 @@ class historicalSurface : public vTempWindow
     vQueue getSurface(int queryTime, int queryWindow, int d);
     vQueue getSurface(int queryTime, int queryWindow, int d, int x, int y);
     vQueue getSurface(int queryTime, int queryWindow, int xl, int xh, int yl, int yh);
-
+    void getSurfaceN(ev::vQueue &qret, int queryTime, int numEvents, int d);
+    void getSurfaceN(ev::vQueue &qret, int queryTime, int numEvents, int d, int x, int y);
+    void getSurfaceN(ev::vQueue &qret, int queryTime, int numEvents, int xl, int xh, int yl, int yh);
 
 };
 

diff --git a/libraries/src/vWindow_adv.cpp b/libraries/src/vWindow_adv.cpp
@@ -815,5 +815,48 @@ vQueue historicalSurface::getSurface(int queryTime, int queryWindow, int xl, int
     return qret;
 }
 
+void historicalSurface::getSurfaceN(ev::vQueue &qret, int queryTime, int numEvents, int d)
+{
+    if(q.empty()) return; // vQueue();
+
+    auto v = is_event<AE>(q.back());
+    return getSurfaceN(qret, queryTime, numEvents, d, v->x, v->y);
+}
+
+void historicalSurface::getSurfaceN(ev::vQueue &qret, int queryTime, int numEvents, int d, int x, int y)
+{
+    if(q.empty()) return; // vQueue();
+    return getSurfaceN(qret, queryTime, numEvents, x - d, x + d, y - d, y + d);
+}
+
+void historicalSurface::getSurfaceN(ev::vQueue &qret, int queryTime, int numEvents, int xl, int xh, int yl, int yh)
+{
+    if(q.empty()) return; // vQueue();
+
+//    vQueue qret;
+    int ctime = q.back()->stamp;
+    int countEvents = 0;
+    surface.zero();
+
+    for(vQueue::reverse_iterator qi = q.rbegin(); qi != q.rend(); qi++) {
+        auto v = is_event<AE>(*qi);
+
+        if(surface(v->x, v->y)) continue;
+
+        int cdeltat = ctime - v->stamp;
+        if(cdeltat < 0) cdeltat += vtsHelper::max_stamp;
+        if(cdeltat < queryTime) continue;
+
+        surface(v->x, v->y) = 1;
+        if(v->x >= xl && v->x <= xh && v->y >= yl && v->y <= yh) {
+            qret.push_back(*qi);
+            countEvents++;
+        }
+
+        if(countEvents > numEvents) break;
+    }
+//    return qret;
+}
+
 
 }