Merge pull request #46 from pmacalpine/drawing_examples

Updating drawing examples with agent selection demo, closes #40
magmaOffenburg · Jun 3, 2015 · 542fd4f · 542fd4f
2 parents efe050e + 602ad2b
commit 542fd4f
Show file tree

Hide file tree

Showing 4 changed files with 102 additions and 1 deletion.
diff --git a/examples/cpp/rvdraw.h b/examples/cpp/rvdraw.h
@@ -186,7 +186,7 @@ unsigned char* newAnnotation(const string* text, const float* p,
 unsigned char* newAgentAnnotation(const string* text, bool leftTeam,
     int agentNum, const float* color, int* bufSize) {
 
-  *bufSize = (text == NULL) ? 3 : 7 + text->length;
+  *bufSize = (text == NULL) ? 3 : 7 + text->length();
   unsigned char* buf = new unsigned char[*bufSize];
 
   long i = 0;
@@ -205,4 +205,17 @@ unsigned char* newAgentAnnotation(const string* text, bool leftTeam,
   return buf;
 }
 
+unsigned char* newSelectAgent(bool leftTeam, int agentNum, int* bufSize) {
+
+  *bufSize = 3;
+  unsigned char* buf = new unsigned char[*bufSize];
+
+  long i = 0;
+  i += writeCharToBuf(buf+i, 3);
+  i += writeCharToBuf(buf+i, 0);
+  i += writeCharToBuf(buf+i, (leftTeam ? agentNum - 1 : agentNum + 127));
+
+  return buf;
+}
+
 #endif
diff --git a/examples/cpp/rvtester.cpp b/examples/cpp/rvtester.cpp
@@ -32,6 +32,7 @@
 #include <netdb.h>
 #include <string>
 #include <math.h>
+#include <sstream> 
 #include "rvdraw.h"
 
 using namespace std;
@@ -114,6 +115,23 @@ void drawAnnotation(const string* text, float x, float y, float z, float r,
   delete[] buf;
 }
 
+void drawAgentAnnotation(const string* text, bool leftTeam, int agentNum, 
+    float r, float g, float b) {
+  float color[3] = {r,g,b};
+
+  int bufSize = -1;
+  unsigned char* buf = newAgentAnnotation(text, leftTeam, agentNum, color, &bufSize);
+  sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen);
+  delete[] buf;
+}
+
+void selectAgent(bool leftTeam, int agentNum) {
+  int bufSize = -1;
+  unsigned char *buf = newSelectAgent(leftTeam, agentNum, &bufSize);
+  sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen);
+  delete[] buf;
+}
+
 float maxf(float a, float b) {
   return a > b ? a : b;
 }
@@ -181,12 +199,46 @@ void renderStaticShapes() {
   swapBuffers(&staticSets);
 }
 
+void annotateAgents() {
+  // Annotate left team agents
+  for (int i = 1; i <= 11; i++) {
+    ostringstream stream;
+    stream << "L" << i;
+    const string annotation = stream.str(); 
+    drawAgentAnnotation(&annotation, true, i, 0, 0, 1); 
+  }
+
+  // Annotate right team agents
+  for (int i = 1; i <= 11; i++) {
+    ostringstream stream;
+    stream << "R" << i;
+    const string annotation = stream.str(); 
+    drawAgentAnnotation(&annotation, false, i, 1, 0, 0); 
+  }  
+}
+
+void selectAgents() {
+  // Select left team agents
+  for (int i = 1; i <= 11; i++) {
+    selectAgent(true, i); 
+    usleep(500000);
+  }
+
+  // Select right team agents
+  for (int i = 1; i <= 11; i++) {
+    selectAgent(false, i); 
+    usleep(500000);
+  } 
+}
+
 void runTest() {
   renderStaticShapes();
   for (int i = 0; i < TEST_DURATION / 17; i++) {
     renderAnimatedShapes();
     usleep(16000);
   }
+  annotateAgents();
+  selectAgents();
 }
 
 int main(int argc, char *argv[])

diff --git a/examples/java/RVDraw.java b/examples/java/RVDraw.java
@@ -264,4 +264,19 @@ public static byte[] newAgentAnnotation(String text, boolean leftTeam,
 
         return buf.array();
     }
+
+    /**
+     * Selects the agent with the given team and uniform number. 
+     */
+    public static byte[] newSelectAgent(boolean leftTeam, int agentNum) {
+
+        int numBytes = 3;
+        ByteBuffer buf = ByteBuffer.allocate(numBytes);
+
+        buf.put((byte) 3);
+        buf.put((byte) 0);
+        buf.put((byte)(leftTeam ? agentNum - 1 : agentNum + 127));
+
+        return buf.array();
+    }
 }
diff --git a/examples/java/RVTester.java b/examples/java/RVTester.java
@@ -131,6 +131,21 @@ private void renderStaticShapes() throws IOException {
         swapBuffers("static");
     }
 
+    /** Method for selecting all agents */
+    private void selectAgents() throws Exception {
+        // Select left team agents
+        for (int i = 1; i <= 11; i++) {
+            selectAgent(true, i);
+            Thread.sleep(500);
+        }
+
+        // Select right team agents
+        for (int i = 1; i <= 11; i++) {
+            selectAgent(false, i);
+            Thread.sleep(500);
+        }
+    }
+
     public void runTest() throws IOException {
         animationTimer.start();
         renderStaticShapes();
@@ -183,11 +198,17 @@ public void drawAgentAnnotation(String text, boolean leftTeam,
         socket.send(new DatagramPacket(buf, buf.length, address, ROBOVIS_PORT));
     }
 
+    public void selectAgent(boolean leftTeam, int agentNum) throws IOException {
+        byte[] buf = RVDraw.newSelectAgent(leftTeam, agentNum);
+        socket.send(new DatagramPacket(buf, buf.length, address, ROBOVIS_PORT));
+    }
+
     public static void main(String[] args) throws Exception {
         RVTester tester = new RVTester();
         tester.runTest();
         Thread.sleep(TEST_DURATION);
         tester.drawAgentAnnotation(null, true, 1, Color.CYAN);
         tester.animationTimer.stop();
+        tester.selectAgents();
     }
 }