Skip to content

GitLab

Commit c8defd13 authored by Timothy Stack's avatar Timothy Stack
Browse files
Options

More tweaks and some comments.

parent 443582dc
Show whitespace changes
Inline Side-by-side
Showing with 209 additions and 63 deletions
robots/vmcd/visionTrack.c
View file @ c8defd13
......@@ -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)
......@@ -139,6 +132,10 @@ void vtCoalesce(struct lnMinList *extra,
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;
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;
struct vision_track *vt_extra;
float distance;
printf("try coalesce %f %f -- %f %f %d %d\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)) {
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);
lnRemove(&vt_extra->vt_link);
lnAddHead(extra, &vt_extra->vt_link);
}
vt_extra = vt_succ;
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;
}
......
robots/vmcd/visionTrack.h
View file @ c8defd13
......@@ -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);
......
robots/vmcd/vmcd.c
View file @ c8defd13
......@@ -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,
&current_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, &current_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)) {
......@@ -418,17 +458,31 @@ 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, &current_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;
......@@ -525,7 +602,12 @@ int main(int argc, char *argv[])
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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment