void vtAgeTracks(struct lnMinList *now,
struct lnMinList *old,
struct lnMinList *pool)
{
struct vision_track *vt;
assert(now != NULL);
assert(old != NULL);
while ((vt = (struct vision_track *)lnRemHead(old)) != NULL) {
if (vt->vt_age < MAX_TRACK_AGE) {
float distance = MERGE_TOLERANCE;
if (vtFindMin(vt,
(struct vision_track *)now->lh_Head,
&distance) == NULL) {
vt->vt_age += 1;
lnAddHead(now, &vt->vt_link);
vt = NULL;
}
}
if (vt != NULL)
lnAddHead(pool, &vt->vt_link);
}
}
static void *mypcap_reader(void *pcap)
{
struct lnMinNode *next_node = NULL;
pcap_t *p = pcap;
while (1) {
int cc, idx;
pthread_mutex_lock(&mypcap_data.mpd_mutex);
if (next_node != NULL) {
lnAddTail(&mypcap_data.mpd_buflist, next_node);
pthread_cond_signal(&mypcap_data.mpd_cond);
}
if ((next_node = lnRemHead(&mypcap_data.mpd_freelist))
== NULL) {
next_node = lnRemHead(&mypcap_data.mpd_buflist);
}
pthread_mutex_unlock(&mypcap_data.mpd_mutex);
idx = next_node - mypcap_data.mpd_nodes;
again:
cc = read(p->fd,
(char *)mypcap_data.mpd_buffer[idx],
p->bufsize);
if (cc < 0) {
/* Don't choke when we get ptraced */
switch (errno) {
case EINTR:
goto again;
case EWOULDBLOCK:
return (0);
}
snprintf(p->errbuf, PCAP_ERRBUF_SIZE, "read: %s",
pcap_strerror(errno));
return NULL;
}
mypcap_data.mpd_buflen[idx] = cc;
}
}
void vtCopyTracks(struct lnMinList *dst,
struct lnMinList *src,
struct lnMinList *pool)
{
struct vision_track *vt;
assert(dst != NULL);
assert(src != NULL);
assert(pool != NULL);
vt = (struct vision_track *)src->lh_Head;
while (vt->vt_link.ln_Succ != NULL) {
struct vision_track *vt_copy = (struct vision_track *)lnRemHead(pool);
*vt_copy = *vt;
lnAddTail(dst, &vt_copy->vt_link);
vt = (struct vision_track *)vt->vt_link.ln_Succ;
}
}
int mypcap_read(pcap_t *pd, void *user)
{
static struct lnMinNode *node = NULL;
int idx, cc, retval = 0;
char *bp, *ep;
pthread_mutex_lock(&mypcap_data.mpd_mutex);
if (node != NULL) {
lnAddHead(&mypcap_data.mpd_freelist, node);
}
while ((node = lnRemHead(&mypcap_data.mpd_buflist)) == NULL) {
pthread_cond_wait(&mypcap_data.mpd_cond,
&mypcap_data.mpd_mutex);
}
pthread_mutex_unlock(&mypcap_data.mpd_mutex);
idx = node - mypcap_data.mpd_nodes;
bp = mypcap_data.mpd_buffer[idx];
cc = mypcap_data.mpd_buflen[idx];
/*
* Loop through each packet.
*/
#define bhp ((struct bpf_hdr *)bp)
ep = bp + cc;
while (bp < ep) {
register int caplen, hdrlen;
caplen = bhp->bh_caplen;
hdrlen = bhp->bh_hdrlen;
/*
* XXX A bpf_hdr matches a pcap_pkthdr.
*/
(*mypcap_data.mpd_callback)(user,
(struct pcap_pkthdr*)bp,
bp + hdrlen);
bp += BPF_WORDALIGN(caplen + hdrlen);
}
#undef bhp
return retval;
}
pp_plot_code_t pp_plot_waypoint(struct path_plan *pp)
{
struct lnMinList dextra, sextra, intersections;
float distance, cross, theta;
pp_plot_code_t retval;
int in_ob = 0;
assert(pp != NULL);
if (debug > 0) {
info(" plot %.2f %.2f -> %.2f %.2f\n",
pp->pp_actual_pos.x, pp->pp_actual_pos.y,
pp->pp_goal_pos.x, pp->pp_goal_pos.y);
}
lnNewList(&dextra);
lnNewList(&sextra);
lnNewList(&intersections);
retval = PPC_NO_WAYPOINT; /* if nothing else */
/*
* We have to iterate over one or more waypoints, so we start with the
* Initial waypoint is the goal point.
*/
pp->pp_waypoint = pp->pp_goal_pos;
do {
struct obstacle_node *on, *min_on = NULL;
int goal_in_ob = 0, robot_ob = 0;
float min_distance = FLT_MAX;
/*
* The path doesn't cross enough of the obstacle box for us to worry
* about it so we dump it on the extra list so the intersect doesn't
* return it again. XXX The number is partially a guess and
* empirically derived.
*/
cross = PP_MIN_OBSTACLE_CROSS;
/*
* First we find all of the obstacles that intersect with this path
* and locate the minimum distance to an obstacle.
*/
if (debug)
info("wp %.2f %.2f\n", pp->pp_waypoint.x, pp->pp_waypoint.y);
while ((on = ob_find_intersect2(&pp->pp_actual_pos,
&pp->pp_waypoint,
&distance,
&cross)) != NULL) {
#if 0
info("intr %d %.2f %.2f\n",
on->on_natural.id,
distance,
cross);
#endif
if (on->on_type == OBT_ROBOT)
robot_ob = 1;
switch (pp_point_identify(&pp->pp_actual_pos, &on->on_expanded)) {
case PPT_INTERNAL:
case PPT_CORNERPOINT:
case PPT_OUTOFBOUNDS:
in_ob = 1;
break;
default:
break;
}
if (rc_compute_code(pp->pp_goal_pos.x,
pp->pp_goal_pos.y,
&on->on_expanded) == 0) {
goal_in_ob = 1;
}
lnRemove(&on->on_link);
/* Record the intersection and */
lnAddTail(&intersections, &on->on_link);
/* ... check if this is the closest obstacle. */
if (distance < min_distance) {
min_distance = distance;
min_on = on;
}
cross = PP_MIN_OBSTACLE_CROSS;
}
/* Check if there was an actual intersection. */
if (min_on != NULL) {
if (debug > 1)
info("closest %d\n", min_on->on_natural.id);
pp->pp_flags |= PPF_HAS_OBSTACLE;
lnRemove(&min_on->on_link); // from the intersections list
lnAddTail(min_on->on_type == OBT_STATIC ? &sextra : &dextra,
&min_on->on_link);
pp->pp_obstacle = min_on->on_expanded;
/*
* Find all obstacles that the path intersects with and overlap the
* closest one. The rest of the obstacles will be dealt with after
* we get around the closest one.
*/
while ((on = (struct obstacle_node *)
lnRemHead(&intersections)) != NULL) {
if (rc_rect_intersects(&pp->pp_obstacle, &on->on_expanded)) {
if (debug > 1)
info("merging %d\n", on->on_natural.id);
ob_merge_obstacles(&pp->pp_obstacle, &on->on_expanded);
}
lnAddTail(on->on_type == OBT_STATIC ? &sextra : &dextra,
&on->on_link);
}
if (goal_in_ob) {
struct robot_position rp;
struct rc_line rl;
float r;
mtp_polar(&pp->pp_goal_pos, &pp->pp_actual_pos, &r, &theta);
if (robot_ob)
theta += M_PI_2;
mtp_cartesian(&pp->pp_goal_pos, r + 1000.0, theta, &rp);
rl.x0 = pp->pp_goal_pos.x;
rl.y0 = pp->pp_goal_pos.y;
rl.x1 = rp.x;
rl.y1 = rp.y;
rc_clip_line(&rl, &pp->pp_obstacle);
pp->pp_waypoint.x = rl.x1;
pp->pp_waypoint.y = rl.y1;
if (pp_point_distance(&pp->pp_waypoint,
&pp->pp_actual_pos) > 0.075) {
if (pp_point_reachable(pp, &pp->pp_waypoint))
retval = PPC_WAYPOINT;
else if (pp_next_cornerpoint(pp))
retval = PPC_WAYPOINT;
else
retval = PPC_BLOCKED;
}
else {
retval = PPC_GOAL_IN_OBSTACLE;
}
}
else if (pp_next_cornerpoint(pp)) {
retval = PPC_WAYPOINT;
}
else {
retval = PPC_BLOCKED;
}
}
/* Restore the active obstacle list. XXX Shouldn't do this here. */
lnPrependList(&ob_data.od_active, &dextra); // dynamics first
lnAppendList(&ob_data.od_active, &sextra); // statics last
/* Check for obstacles on the path to our new waypoint. */
cross = PP_MIN_OBSTACLE_CROSS;
} while ((retval == PPC_WAYPOINT) &&
(ob_find_intersect2(&pp->pp_actual_pos,
&pp->pp_waypoint,
&distance,
&cross) != NULL) &&
!in_ob);
/* And make sure it is still sane. */
assert(ob_data_invariant());
/* restrict final waypoint to MAX_DISTANCE */
mtp_polar(&pp->pp_actual_pos,
(retval == PPC_WAYPOINT) ? &pp->pp_waypoint : &pp->pp_goal_pos,
&distance,
&theta);
if (distance > pp_data.ppd_max_distance) {
mtp_cartesian(&pp->pp_actual_pos,
pp_data.ppd_max_distance,
theta,
&pp->pp_waypoint);
retval = PPC_WAYPOINT;
}
switch (retval) {
case PPC_WAYPOINT:
info("set waypoint(%s) %.2f %.2f\n",
pp->pp_robot->hostname,
pp->pp_waypoint.x,
pp->pp_waypoint.y);
break;
case PPC_NO_WAYPOINT:
info("set waypoint(%s) NONE\n", pp->pp_robot->hostname);
break;
case PPC_BLOCKED:
info("set waypoint(%s) BLOCKED\n", pp->pp_robot->hostname);
break;
case PPC_GOAL_IN_OBSTACLE:
info("set waypoint(%s) GOAL_IN_OBSTACLE\n", pp->pp_robot->hostname);
break;
}
return retval;
}
int vtUpdate(struct lnMinList *now,
struct vmc_client *vc,
struct mtp_packet *mp,
struct lnMinList *pool)
{
int retval = 0;
assert(now != NULL);
assert(vc != NULL);
assert(mp != NULL);
assert(pool != NULL);
if (debug > 2) {
mtp_print_packet(stdout, mp);
fflush(stdout);
}
switch (mp->data.opcode) {
case MTP_CONTROL_ERROR:
retval = 1;
break;
case MTP_UPDATE_POSITION:
{
struct mtp_update_position *mup;
struct vision_track *vt;
mup = &mp->data.mtp_payload_u.update_position;
vt = (struct vision_track *)lnRemHead(pool);
assert(vt != NULL);
vt->vt_position = mup->position;
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. */
if (mup->position.x < vc->vc_left)
vc->vc_left = mup->position.x;
if (mup->position.x > vc->vc_right)
vc->vc_right = mup->position.x;
if (mup->position.y < vc->vc_top)
vc->vc_top = mup->position.y;
if (mup->position.y > vc->vc_bottom)
vc->vc_bottom = mup->position.y;
retval = (mup->status == MTP_POSITION_STATUS_CYCLE_COMPLETE);
}
break;
default:
error("unhandled vmc-client packet %d\n", mp->data.opcode);
break;
}
return retval;
}
