Finally, the smoothing (via moving average) changes have been tested

enough and seem to work.  Currently, the moving average window size is set
to 5 frames (in testbed/event/sched/event-sched.c); a window size of 10
produced too much position lag.

event/sched/event-sched.c

@@ -159,6 +159,9 @@ static void sighup(int sig)
 		execlp("vmcd",
 		       "vmcd",
 		       "-d",
+		       "-S",
+		       "-w",
+		       "5",
 		       "-l",
 		       "logs/vmcd.log",
 		       "-U",
@@ -457,6 +460,9 @@ main(int argc, char *argv[])
 			execlp("vmcd",
 			       "vmcd",
 			       "-d",
+			       "-S",
+			       "-w",
+			       "5",
 			       "-l",
 			       "logs/vmcd.log",
 			       "-U",

robots/vmcd/visionTrack.c

@@ -17,6 +17,7 @@
 
 #include "log.h"
 
+#include "robotObject.h"
 #include "visionTrack.h"
 
 extern int debug;
@@ -182,6 +183,15 @@ int vtUpdate(struct lnMinList *now,
 	    vt->vt_client = vc;
 	    vt->vt_age = 0;
 	    vt->vt_userdata = NULL;
+
+
+	    /* zero out the smoothing data for this 
+	     * track!
+	     */
+	    //vt->ma.oldest_index = 0;
+	    //vt->ma.number_valid_positions = 0;
+
+
 	    lnAddTail(now, &vt->vt_link);
 
 	    /* Adjust the cameras viewable area based on this track. */
@@ -376,6 +386,8 @@ void vtMatch(struct lnMinList *pool,
 				 &distance)) != NULL) {
 	    if (distance <= 0.06) {
 		vt->vt_userdata = vt_prev->vt_userdata;
+		//info("COPIED old moving avg data\n");
+		vt->ma = vt_prev->ma;
 		lnRemove(&vt_prev->vt_link);
 		lnAddHead(pool, &vt_prev->vt_link);
 	    }
@@ -476,3 +488,113 @@ struct vision_track *vtFindWiggle(struct lnMinList *start,
     
     return retval;
 }
+
+ 
+/** 
+ * Given a moving average, updates it with the data in new and outputs the 
+ * calculated average in avg.
+ * 
+ * @param ma The moving average metadata.
+ * @param new The data to add to the moving average.
+ * @param avg The output from this function; contains the latest moving avg.
+ */
+static void updateMovingAverage(struct moving_average *ma,
+				struct robot_position *new) {
+    int i;
+    int vp;
+    
+    assert(ma != NULL);
+    assert(ma->positions != NULL);
+    assert(new != NULL);
+    
+    /* update data samples array */
+    ma->positions[ma->oldest_index] = *new;
+    /* point to the next piece of data to be replaced */
+    ma->oldest_index = (ma->oldest_index + 1) % ma->positions_len;
+    /* if the ring buffer isn't full yet, incr the number of valid data pts */
+    if (ma->number_valid_positions < ma->positions_len) {
+	++(ma->number_valid_positions);
+    }
+    
+    ma->current_avg.x = 0.0f;
+    ma->current_avg.y = 0.0f;
+    ma->current_avg.theta = 0.0f;
+    
+    vp = ma->number_valid_positions;
+    for (i = 0; i < vp; ++i) {
+	ma->current_avg.x += ma->positions[i].x;
+	ma->current_avg.y += ma->positions[i].y;
+	ma->current_avg.theta += ma->positions[i].theta;
+    }
+    
+    if (debug > 2) {
+	info("UPDATING moving avg: %d valid positions\n",vp);
+    }
+
+    ma->current_avg.x = ma->current_avg.x / vp;
+    ma->current_avg.y = ma->current_avg.y / vp;
+    ma->current_avg.theta = ma->current_avg.theta / vp;
+    
+    return;
+}
+
+void vtSmooth(struct lnMinList *tracks) {
+    struct vision_track *vt = NULL;
+    struct robot_object *ro = NULL;
+    struct moving_average *ma = NULL;
+    float distance_delta = 0.0f;
+    float theta_delta = 0.0f;
+    
+    assert(tracks != NULL);
+    
+    vt = (struct vision_track *)tracks->lh_Head;
+    while (vt->vt_link.ln_Succ != NULL) {
+	if ((ro = vt->vt_userdata) != NULL) {
+	    ma = &(vt->ma);
+	    
+	    /* using the latest position in ro->position, calc moving avg */
+	    updateMovingAverage(ma,
+				&(vt->vt_position));
+
+	    if (debug > 2) {
+		info("SMOOTHED position for robot %d from (%f,%f,%f) to "
+		     "(%f,%f,%f)\n",
+		     ro->ro_id,
+		     vt->vt_position.x,
+		     vt->vt_position.y,
+		     vt->vt_position.theta,
+		     ma->current_avg.x,
+		     ma->current_avg.y,
+		     ma->current_avg.theta
+		     );
+	    }
+
+	    /* figure out how much the posit will change from the last
+	     * one reported... this is important so we know if we mess up
+	     * wiggles
+	     */
+	    distance_delta = hypotf(vt->vt_position.x - ma->current_avg.x,
+				    vt->vt_position.y - ma->current_avg.y);
+	    theta_delta = (float)abs(vt->vt_position.theta - 
+				     ma->current_avg.theta);
+	    if (debug > 2) {
+		info("  DELTAS were %f dist and %f angle.\n",
+		     distance_delta,
+		     theta_delta);
+	    }
+
+	    /*  update the position... this is a bit sneaky... but this way
+	     * we don't have to change oodles of code.
+	     */
+	    vt->vt_position.x = ma->current_avg.x;
+	    vt->vt_position.y = ma->current_avg.y;
+	    vt->vt_position.theta = ma->current_avg.theta;
+	    // don't update the timestamp!
+	    
+	}
+	
+	vt = (struct vision_track *)vt->vt_link.ln_Succ;
+    }
+    
+    return;
+}

robots/vmcd/visionTrack.h

@@ -34,6 +34,18 @@
  * lost.
  */
 #define MAX_TRACK_AGE 5
+/**
+ * Structure used to keep a moving average of last N positions.
+ */
+struct moving_average {
+    struct robot_position *positions;  /*< ring buffer of most recent posits */
+    int positions_len;                 /*< length of buffer */
+    int number_valid_positions;        /*< the buffer may not be full... */
+    
+    /* don't want to shift the contents of the buffer every time, doh */
+    int oldest_index;                  /*< this is the one we should replace */
+    struct robot_position current_avg; /*< the last calc'd avg */
+};
 
 /**
  * The maximum distance, in meters, that will be tolerated between an initial
@@ -56,6 +68,7 @@ struct vision_track {
     struct vmc_client *vt_client;	/*< Camera that detected the track. */
     int vt_age;				/*< Age of the track (lower=younger) */
     void *vt_userdata;			/*< Data attached to the track. */
+    struct moving_average ma;           /*< smooth out the positions... */
 };
 
 /**
@@ -152,4 +165,12 @@ void vtUnknownTracks(struct lnMinList *unknown, struct lnMinList *mixed);
 struct vision_track *vtFindWiggle(struct lnMinList *start,
 				  struct lnMinList *now);
 
+/**
+ * Taking the latest position, augment the moving average for each track.  This
+ * should be done AFTER a vtMatch so that we only avg on valid data.  Doing 
+ * smoothing after everything else will allow the current matching algorithms 
+ * to continue to work.
+ */
+void vtSmooth(struct lnMinList *tracks);
+
 #endif

robots/vmcd/vmcd.c

@@ -74,6 +74,13 @@ static struct lnMinList last_frame, current_frame, wiggle_frame;
 
 static struct robot_object *wiggle_bot = NULL;
 
+#define DEFAULT_SMOOTH_WINDOW 10
+ 
+/* by default, smoothing is off */
+static int smoothing = 1;
+static int smoothing_window = DEFAULT_SMOOTH_WINDOW;
+
+
 /**
  * Here's how the algorithm will lay out: when we get update_position packets
  * from a vmc-client, we will attempt to find a preexisting track for the given
@@ -247,20 +254,24 @@ static void dump_info(void)
 static void usage(void)
 {
     fprintf(stderr,
-            "Usage: vmcd [-hd] [-l logfile] [-e emchost] [-p emcport]\n"
-            "            [-c clienthost] [-P clientport]\n"
+            "Usage: vmcd [-hdS] [-l logfile] [-e emchost] [-p emcport]\n"
+            "            [-c clienthost] [-P clientport] [-s #samples]\n"
             "Vision master control daemon\n"
             "  -h\tPrint this message\n"
             "  -d\tTurn on debugging messages and do not daemonize\n"
+	    "  -S\tTurn on position smoothing\n"
             "  -l logfile\tSpecify the log file to use\n"
             "  -i pidfile\tSpecify the pid file name\n"
             "  -e emchost\tSpecify the host name where emc is running\n"
             "  -p emcport\tSpecify the port that emc is listening on\n"
             "  -c clienthost\tSpecify the host name where a vmc-client is\n"
             "  -P clientport\tSpecify the port the client is listening on\n"
+	    "  -w #samples\tSmooth over the last #samples positions (default "
+	    "             \tis %d)\n"
             "\n"
             "Example:\n"
-            "  $ vmcd -c foo -P 7070 -- -c foo -P 7071\n");
+            "  $ vmcd -c foo -P 7070 -- -c foo -P 7071\n",
+	    DEFAULT_SMOOTH_WINDOW);
 }
 
 static int parse_client_options(int *argcp, char **argvp[])
@@ -274,8 +285,22 @@ static int parse_client_options(int *argcp, char **argvp[])
     argc = *argcp;
     argv = *argvp;
     
-    while ((c = getopt(argc, argv, "hdp:U:l:i:e:c:P:")) != -1) {
+    while ((c = getopt(argc, argv, "hdSp:U:l:i:e:c:P:w:")) != -1) {
         switch (c) {
+	case 'S':
+	    smoothing = 1;
+	    fprintf(stdout,"DATA WILL BE SMOOTHED!\n");
+	    break;
+	case 'w':
+	    smoothing_window = atoi(optarg);
+	    if (smoothing_window < 2) {
+		fprintf(stdout,
+			"WARNING: smoothing window must be at least 2; setting"
+			" to default of %d.\n",
+			DEFAULT_SMOOTH_WINDOW);
+		smoothing_window = DEFAULT_SMOOTH_WINDOW;
+	    }
+	    break;
         case 'h':
             usage();
             exit(0);
@@ -364,6 +389,14 @@ static int wiggle_complete(struct robot_object *ro, mtp_packet_t *mp)
 	roMoveRobot(ro, ROS_LOST);
     }
     else {
+
+	/* zero out the smoothing data for this 
+	 * track!
+	 */
+	info("WC: RESET ma data\n");
+	vt->ma.oldest_index = 0;
+	vt->ma.number_valid_positions = 0;
+
 	vt->vt_userdata = wiggle_bot;
 	roMoveRobot(ro, ROS_KNOWN);
     }
@@ -462,6 +495,21 @@ int main(int argc, char *argv[])
         retval = parse_client_options(&argc, &argv);
     } while ((argc > 0) && strcmp(argv[0], "--") == 0);
     
+ 
+    /* set up smoothing */
+    if (smoothing) {
+	
+	for (lpc = 0; lpc < TRACK_POOL_SIZE; ++lpc) {
+	    vt_pool_nodes[lpc].ma.positions = (struct robot_position *)
+		malloc(sizeof(struct robot_position)*smoothing_window);
+	    bzero(vt_pool_nodes[lpc].ma.positions,
+		  sizeof(struct robot_position)*smoothing_window);
+	    vt_pool_nodes[lpc].ma.positions_len = smoothing_window;
+	    vt_pool_nodes[lpc].ma.number_valid_positions = 0;
+	    vt_pool_nodes[lpc].ma.oldest_index = 0;
+	}
+    }
+
     if (debug) {
         loginit(0, logfile);
     }
@@ -658,6 +706,13 @@ int main(int argc, char *argv[])
 	    /* Match the current frame to the last frame. */
 	    vtMatch(&vt_pool, &last_frame, &current_frame);
 
+
+	    /* (maybe) Smooth the matched bot posits */
+  	    if (smoothing) { 
+  		vtSmooth(&current_frame); 
+	    }
+
+
 	    /* Reset the list of known/unknown bots. */
 	    lnAppendList(&ro_data.rd_lists[ROS_UNKNOWN],
 			 &ro_data.rd_lists[ROS_KNOWN]);