Commit eeaa6aa2 authored by David Johnson's avatar David Johnson

Fixes for 'lost' robots: the vision system will now periodically try to

wiggle 'lost' robots (those for whom a wiggle, or subsequent wiggles, were
not detectable).
parent 94738e27
......@@ -18,6 +18,7 @@ struct robot_object {
unsigned long ro_flags;
char *ro_name;
int ro_id;
struct timeval ro_lost_timestamp;
};
struct robot_object *roFindRobot(struct lnMinList *list, int id);
......
......@@ -35,12 +35,14 @@ static struct vision_track *vtFindMin(struct vision_track *vt,
distance = hypotf(vt->vt_position.x - curr->vt_position.x,
vt->vt_position.y - curr->vt_position.y);
#if 0
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);
#endif
if (distance < *distance_out) {
retval = curr;
*distance_out = distance;
......@@ -97,14 +99,14 @@ int vtUpdate(struct lnMinList *now,
if (mup->position.y > vc->vc_bottom)
vc->vc_bottom = mup->position.y;
#if 0
printf("vc %p %f %f %f %f\n",
vc,
vc->vc_left,
vc->vc_right,
vc->vc_top,
vc->vc_bottom);
#endif
retval = (mup->status == MTP_POSITION_STATUS_CYCLE_COMPLETE);
}
break;
......
......@@ -445,8 +445,12 @@ int main(int argc, char *argv[])
* 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) {
info("checking through unknown_bots; id %d\n",
ro->ro_id
);
if (!(ro->ro_flags & RF_WIGGLING)) {
struct mtp_packet wmp;
......@@ -478,6 +482,44 @@ int main(int argc, char *argv[])
current_client = 0;
}
/*
* Check which bots in the lost list have exceeded the timeout;
* if any have, add them back to the unknown_bots list, and they
* will be rewiggled once the next set of updates comes from the
* vision system.
*/
if (!lnEmptyList(&lost_bots)) {
struct robot_object *ro = NULL;
struct robot_object *ro_next = NULL;
struct timeval current;
gettimeofday(&current,NULL);
ro = (struct robot_object *)lost_bots.lh_Head;
while (ro->ro_link.ln_Succ != NULL) {
if ((current.tv_sec - ro->ro_lost_timestamp.tv_sec) > 10) {
// remove this guy
ro_next = (struct robot_object *)ro->ro_link.ln_Succ;
lnRemove(&ro->ro_link);
// add him to unknown list
lnAddTail(&unknown_bots,&ro->ro_link);
info("robot %d exceeded the lost timeout, trying to "
"reacquire.\n",
ro->ro_id
);
// also have to unset the RF_WIGGLING flag
ro->ro_flags &= ~RF_WIGGLING;
ro = ro_next;
}
else {
//info("robot %d did not exceed the lost timeout!\n",
// ro->ro_id
// );
ro = (struct robot_object *)ro->ro_link.ln_Succ;
}
}
}
/*
* Check if we need to identify any bots, assuming we're not in the
* process of finding one.
......@@ -564,6 +606,14 @@ int main(int argc, char *argv[])
*/
lnAddTail(&lost_bots,
&wiggle_bot->ro_link);
gettimeofday(&(wiggle_bot->ro_lost_timestamp),
NULL);
info("took a lost timestamp for robot"
" %d\n",
wiggle_bot->ro_id
);
wiggle_bot = NULL;
}
else {
......@@ -608,9 +658,11 @@ int main(int argc, char *argv[])
}
if (vtUpdate(&current_frame, vc, &mp, &vt_pool)) {
printf("got frame from client %s:%d\n",
vc->vc_hostname,
vc->vc_port);
if (debug > 1) {
printf("got frame from client %s:%d\n",
vc->vc_hostname,
vc->vc_port);
}
/*
* Got all of the tracks from this client, clear
......
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