From c8defd13260a5e61172751b222cfb57944521d64 Mon Sep 17 00:00:00 2001 From: Timothy Stack Date: Tue, 18 Jan 2005 16:17:02 +0000 Subject: [PATCH] More tweaks and some comments. --- robots/vmcd/visionTrack.c | 111 +++++++++++++++++++++---------------- robots/vmcd/visionTrack.h | 49 +++++++++++++++++ robots/vmcd/vmcd.c | 112 +++++++++++++++++++++++++++++++++----- 3 files changed, 209 insertions(+), 63 deletions(-) diff --git a/robots/vmcd/visionTrack.c b/robots/vmcd/visionTrack.c index 7c60ad6e3..7a5237a80 100644 --- a/robots/vmcd/visionTrack.c +++ b/robots/vmcd/visionTrack.c @@ -9,46 +9,38 @@ #include "visionTrack.h" -static int vtCompare(struct vision_track *vt1, - struct vision_track *vt2, - float tolerance) -{ - float distance; - int retval; - - assert(vt1 != NULL); - assert(vt2 != NULL); - - distance = hypotf(vt1->vt_position.x - vt2->vt_position.x, - vt1->vt_position.y - vt2->vt_position.y); - - retval = (distance < tolerance); - - if (!retval) { - printf("miss %f %f\n", distance, tolerance); - } - - return retval; -} - +/** + * Find the track in a list that is the minimum distance from a given track. + * + * @param vt The track to compare against the other tracks in the list. + * @param curr The track in a list where the search should start. The function + * will continue the search until it reaches the end of the list. + * @param distance_out Reference to a float where the minimum distance found + * should be stored. + */ static struct vision_track *vtFindMin(struct vision_track *vt, - struct lnMinList *list, + struct vision_track *curr, float *distance_out) { - struct vision_track *curr, *retval = NULL; + struct vision_track *retval = NULL; assert(vt != NULL); - assert(list != NULL); + assert(curr != NULL); assert(distance_out != NULL); *distance_out = FLT_MAX; - curr = (struct vision_track *)list->lh_Head; while (curr->vt_link.ln_Succ != NULL) { float distance; distance = hypotf(vt->vt_position.x - curr->vt_position.x, vt->vt_position.y - curr->vt_position.y); + printf(" min %f %f - %f %f = %f\n", + vt->vt_position.x, + vt->vt_position.y, + curr->vt_position.x, + curr->vt_position.y, + distance); if (distance < *distance_out) { retval = curr; *distance_out = distance; @@ -95,6 +87,7 @@ int vtUpdate(struct lnMinList *now, vt->vt_userdata = NULL; lnAddTail(now, &vt->vt_link); + /* Adjust the cameras viewable area based on this track. */ if (mup->position.x < vc->vc_left) vc->vc_left = mup->position.x; if (mup->position.x > vc->vc_right) @@ -138,7 +131,11 @@ void vtCoalesce(struct lnMinList *extra, vt = (struct vision_track *)now->lh_Head; while (vt->vt_link.ln_Succ != NULL) { int in_camera_count = 0, lpc; - + + /* + * Figure out how many cameras this track might be viewable in so we + * can coalesce them. + */ for (lpc = 0; lpc < vc_len; lpc++) { #if 0 printf("vc2 %f %f -- %f %f %f %f\n", @@ -160,27 +157,23 @@ void vtCoalesce(struct lnMinList *extra, assert(in_camera_count > 0); if (in_camera_count > 1) { - struct vision_track *vt_extra, *vt_succ; + struct vision_track *vt_extra; + float distance; - vt_extra = (struct vision_track *)vt->vt_link.ln_Succ; - while (vt_extra->vt_link.ln_Succ != NULL) { - vt_succ = (struct vision_track *)vt_extra->vt_link.ln_Succ; - - printf("try coalesce %f %f -- %f %f %d %d\n", + while ((in_camera_count > 1) && + ((vt_extra = vtFindMin(vt, + (struct vision_track *) + vt->vt_link.ln_Succ, + &distance)) != NULL) && + (distance < 0.35)) { + printf("coalesce %f %f -- %f %f\n", vt->vt_position.x, vt->vt_position.y, - vt_extra->vt_position.x, vt_extra->vt_position.y, - vt->vt_client->vc_port, vt_extra->vt_client->vc_port); - if ((vt->vt_client != vt_extra->vt_client) && - vtCompare(vt, vt_extra, 0.35)) { - printf("coalesce %f %f -- %f %f\n", - vt->vt_position.x, vt->vt_position.y, - vt_extra->vt_position.x, vt_extra->vt_position.y); - - lnRemove(&vt_extra->vt_link); - lnAddHead(extra, &vt_extra->vt_link); - } + vt_extra->vt_position.x, vt_extra->vt_position.y); - vt_extra = vt_succ; + lnRemove(&vt_extra->vt_link); + lnAddHead(extra, &vt_extra->vt_link); + + in_camera_count -= 1; } } @@ -198,12 +191,18 @@ void vtMatch(struct lnMinList *pool, assert(prev != NULL); assert(now != NULL); + /* + * Walk through the tracks in the current frame trying to find tracks in + * the previous frame, within some threshold. + */ vt = (struct vision_track *)now->lh_Head; while (vt->vt_link.ln_Succ != NULL) { struct vision_track *vt_prev; float distance; - if ((vt_prev = vtFindMin(vt, prev, &distance)) != NULL) { + if ((vt_prev = vtFindMin(vt, + (struct vision_track *)prev->lh_Head, + &distance)) != NULL) { if (distance < 0.30) { vt->vt_userdata = vt_prev->vt_userdata; lnRemove(&vt_prev->vt_link); @@ -214,6 +213,10 @@ void vtMatch(struct lnMinList *pool, vt = (struct vision_track *)vt->vt_link.ln_Succ; } + /* + * Walk through the previous frame copying any young tracks to the current + * frame. + */ vt = (struct vision_track *)prev->lh_Head; while (vt->vt_link.ln_Succ != NULL) { struct vision_track *vt_next; @@ -259,12 +262,22 @@ struct vision_track *vtFindWiggle(struct lnMinList *start, assert(start != NULL); assert(now != NULL); + /* + * Walk through the current frame searching for tracks that have a small + * displacement from the start frame and large orientation change. + */ vt = (struct vision_track *)now->lh_Head; while ((vt->vt_link.ln_Succ != NULL) && (retval == NULL)) { struct vision_track *vt_start; float distance; - - if ((vt_start = vtFindMin(vt, start, &distance)) == NULL) { + + printf("check %f %f %f\n", + vt->vt_position.x, + vt->vt_position.y, + vt->vt_position.theta); + if ((vt_start = vtFindMin(vt, + (struct vision_track *)start->lh_Head, + &distance)) == NULL) { } else if (distance < 0.05) { float diff; @@ -285,6 +298,8 @@ struct vision_track *vtFindWiggle(struct lnMinList *start, printf(" found it %p\n", retval); } } + printf("dist %p %f\n", vt_start, distance); + vt = (struct vision_track *)vt->vt_link.ln_Succ; } diff --git a/robots/vmcd/visionTrack.h b/robots/vmcd/visionTrack.h index fff812bca..0254bcd6b 100644 --- a/robots/vmcd/visionTrack.h +++ b/robots/vmcd/visionTrack.h @@ -15,22 +15,71 @@ struct vision_track { void *vt_userdata; }; +/** + * Update the list of tracks for the current frame with the tracks from a given + * vmc-client. + * + * @param now The track list for the current frame, any new tracks will be + * added to this. + * @param client The reference to the vmc-client that produced a track. + * @param mp The packet received from the client. + * @param pool The pool of nodes to draw vision_track objects from. + * @return True if all of the tracks have been received from this client for + * the current frame. + */ int vtUpdate(struct lnMinList *now, struct vmc_client *client, struct mtp_packet *mp, struct lnMinList *pool); +/** + * Coalesce tracks that are in an area where cameras are overlapping. + * + * @param extra The list to add any duplicate tracks found in the current + * frame. + * @param now The list of tracks from all of the cameras. + * @param vc The array of cameras, coalescing is based on the camera's viewable + * area. + * @param vc_len The length of the vc array. + */ void vtCoalesce(struct lnMinList *extra, struct lnMinList *now, struct vmc_client *vc, unsigned int vc_len); +/** + * Match any tracks from the current frame against the previous frame. Any + * matches will have their vt_userdata value copied over. Any tracks from the + * previous frame that weren't matched, will have their vt_age value + * incremented and be appended to the current frame if their age value is less + * than 5. By keeping older frames around, we can deal with tracks that skip + * a few frames, which seems to happen at the edge of the camera's viewable + * area. + * + * @param pool The pool to add any older, or matched nodes, to. + * @param prev The previous frame to match. + * @param now The current frame. + */ void vtMatch(struct lnMinList *pool, struct lnMinList *prev, struct lnMinList *now); +/** + * Move any tracks whose vt_userdata == NULL from one list to another. + * + * @param unknown The destination for any tracks that have no userdata value. + * @param mixed The list of tracks to sort. + */ void vtUnknownTracks(struct lnMinList *unknown, struct lnMinList *mixed); +/** + * Find a vision_track that changed 180 degrees from a previous frame to the + * current frame. + * + * @param start A snapshot of unknown tracks from before the wiggle started. + * @param now The current frame. + * @param return The matched vision_track or NULL if none could be found. + */ struct vision_track *vtFindWiggle(struct lnMinList *start, struct lnMinList *now); diff --git a/robots/vmcd/vmcd.c b/robots/vmcd/vmcd.c index fb6416603..648a2363b 100644 --- a/robots/vmcd/vmcd.c +++ b/robots/vmcd/vmcd.c @@ -201,8 +201,8 @@ static int parse_client_options(int *argcp, char **argvp[]) int main(int argc, char *argv[]) { + struct lnMinList known_bots, unknown_bots, wiggling_bots, lost_bots; struct lnMinList last_frame, current_frame, wiggle_frame; - struct lnMinList known_bots, unknown_bots, wiggling_bots; int lpc, current_client = 0, retval = EXIT_SUCCESS; struct robot_object *wiggle_bot = NULL; fd_set readfds; @@ -220,6 +220,7 @@ int main(int argc, char *argv[]) lnNewList(&known_bots); lnNewList(&unknown_bots); lnNewList(&wiggling_bots); + lnNewList(&lost_bots); do { retval = parse_client_options(&argc, &argv); @@ -324,6 +325,38 @@ int main(int argc, char *argv[]) } } + /* + * The main loop consists of: + * + * 1. Gathering all of the tracks from the cameras; + * 2. Coalescing the separate tracks into a single "frame" that has all + * of the tracks without any duplicates caused by overlapping cameras. + * 3. Matching the tracks in the current frame to those in the last + * frame, and copying the robot references from the old frame into + * the new frame. + * 4. Sort the list of robots into known and unknown based on whether + * or not they are referenced in the current frame. + * 5. Send MTP_WIGGLE_START to any unknown robots to stop them. + * 6. Move the current frame to the last frame and start reading tracks + * from the cameras again. + * + * Notes: + * + * + We will always get a packet from the vmc-clients, if there are no + * tracks, it will send an error. This behavior allows us to keep all + * of the cameras in sync. + * + * + I don't actually sync the cameras to the same frame, so there might + * be a few frames difference between tracks received from one camera + * and another. + * + * + We read the tracks from each camera in the same order each time so + * the coalesce operation always picks the same track when merging. + * + * + XXX I don't actually do anything with lost_bots at the moment, they + * should be rewiggled after a short timeout. + */ + FD_SET(vmc_clients[0].vc_handle->mh_fd, &readfds); /* handle input from emc and vmc */ @@ -336,25 +369,29 @@ int main(int argc, char *argv[]) int unknown_track_count = 0; struct robot_object *ro; - printf("got whole frame\n"); + /* We've got all of the camera tracks, start processing. */ vtCoalesce(&vt_pool, ¤t_frame, vmc_clients, - vmc_client_count); + vmc_client_count); // Get rid of duplicates + /* + * Match the current frame to the last frame and throw any old + * frames (vt_age > 5) in the pool. + */ vtMatch(&vt_pool, &last_frame, ¤t_frame); + /* Reset the list of known/unknown bots. */ lnAppendList(&unknown_bots, &known_bots); - + + /* + * Detect the robots that have references in the current frame and + * send an MTP_UPDATE_POSITION. All others are left in the + * unknown_bots list. + */ vt = (struct vision_track *)current_frame.lh_Head; while (vt->vt_link.ln_Succ != NULL) { -#if 0 - if (vt->vt_userdata == NULL) { - fake_userdata += 1; - vt->vt_userdata = fake_userdata; - } -#else if ((ro = vt->vt_userdata) != NULL) { struct mtp_packet ump; @@ -373,7 +410,6 @@ int main(int argc, char *argv[]) unknown_track_count += 1; vt_unknown = vt; } -#endif if (debug > 1) { printf("track %f %f %f - %d - %p %s\n", @@ -398,6 +434,10 @@ int main(int argc, char *argv[]) } #endif + /* + * Send MTP_WIGGLE_STARTs for any unknown robots, this should stop + * them and cause an IDLE wiggle_status to come back. + */ ro = (struct robot_object *)unknown_bots.lh_Head; while (ro->ro_link.ln_Succ != NULL) { if (!(ro->ro_flags & RF_WIGGLING)) { @@ -417,18 +457,32 @@ int main(int argc, char *argv[]) } ro = (struct robot_object *)ro->ro_link.ln_Succ; } - + + /* Throw any frames left from the last frame in the pool and */ lnAppendList(&vt_pool, &last_frame); + /* + * ... move the current frame to the last frame for the next + * iteration. + */ lnMoveList(&last_frame, ¤t_frame); FD_SET(vmc_clients[0].vc_handle->mh_fd, &readfds); current_client = 0; } + /* + * Check if we need to identify any bots, assuming we're not in the + * process of finding one. + */ if ((wiggle_bot == NULL) && !lnEmptyList(&wiggling_bots)) { struct mtp_packet wmp; + /* + * Take a snapshot of the tracks with no robot attached from the + * last frame so we can compare it against the frame when the robot + * has finished its wiggle. + */ vtUnknownTracks(&wiggle_frame, &last_frame); wiggle_bot = (struct robot_object *)lnRemHead(&wiggling_bots); mtp_init_packet(&wmp, @@ -460,7 +514,6 @@ int main(int argc, char *argv[]) struct robot_object *ro; float distance; - /* XXX Need to handle update_id msgs here. */ if (debug > 0) { mtp_print_packet(stdout, &mp); } @@ -471,25 +524,49 @@ int main(int argc, char *argv[]) switch (mws->status) { case MTP_POSITION_STATUS_IDLE: + /* Got a response to an MTP_WIGGLE_START. */ if ((ro = roFindRobot(&unknown_bots, mws->robot_id)) == NULL) { fatal("unknown bot %d\n", mws->robot_id); } else { + /* + * Add him to the list of robots ready + * to be wiggled. + */ lnRemove(&ro->ro_link); lnAddTail(&wiggling_bots, &ro->ro_link); } break; case MTP_POSITION_STATUS_COMPLETE: + /* + * The robot finished his wiggle, check the + * current frame against the snapshot we took + * to find a robot in the same position, but + * with a 180 degree difference in orientation. + */ if ((vt = vtFindWiggle(&wiggle_frame, &last_frame)) == NULL) { - fatal("no wiggle found!\n"); + /* + * Couldn't locate the robot, add him to + * the list of "lost" robots which we'll + * try to rediscover after a short time. + */ + lnAddTail(&lost_bots, + &wiggle_bot->ro_link); + wiggle_bot = NULL; } else { info("found wiggle ! %p\n", vt); vt->vt_userdata = wiggle_bot; wiggle_bot->ro_flags &= ~RF_WIGGLING; + lnAddTail(&known_bots, + &wiggle_bot->ro_link); wiggle_bot = NULL; + /* + * Return the frame we snapshotted awhile + * ago to the pool. + */ lnAppendList(&vt_pool, &wiggle_frame); } break; @@ -524,8 +601,13 @@ int main(int argc, char *argv[]) printf("got frame from client %s:%d\n", vc->vc_hostname, vc->vc_port); - + + /* + * Got all of the tracks from this client, clear + * his bit and + */ FD_CLR(vc->vc_handle->mh_fd, &readfds); + /* ... try to wait for the next one. */ current_client += 1; if (current_client < vmc_client_count) FD_SET(vc[1].vc_handle->mh_fd, &readfds); -- GitLab