/** * Extract tracks from a frame that are near the given track. Used when * finding tracks from different cameras that need to be merged. * * @param list_out The list of tracks found to be within the distance * tolerance. * @param vt The track to compare other tracks against. * @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 tolerance The maximum distance that other tracks have to be within * before they are considered "near." * @return The number of tracks in "list_out." */ static int vtExtractNear(struct lnMinList *list_out, struct vision_track *vt, struct vision_track *curr, float tolerance) { int retval = 0; assert(list_out != NULL); assert(vt != NULL); assert(tolerance > 0.0); if (curr == NULL) return retval; while (curr->vt_link.ln_Succ != NULL) { struct vision_track *vt_next; float distance; vt_next = (struct vision_track *)curr->vt_link.ln_Succ; distance = hypotf(vt->vt_position.x - curr->vt_position.x, vt->vt_position.y - curr->vt_position.y); if (distance <= tolerance) { lnRemove(&curr->vt_link); lnAddTail(list_out, &curr->vt_link); retval += 1; } curr = vt_next; } assert(lnEmptyList(list_out) || (retval > 0)); return retval; }
void roMoveRobot(struct robot_object *ro, ro_status_t new_status) { assert(ro != NULL); assert(new_status > ROS_MIN); assert(new_status < ROS_MAX); lnRemove(&ro->ro_link); lnAddTail(&ro_data.rd_lists[new_status], &ro->ro_link); ro->ro_status = new_status; switch (new_status) { case ROS_LOST: gettimeofday(&ro->ro_lost_timestamp, NULL); break; default: break; } }
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; } }
void vtUnknownTracks(struct lnMinList *unknown, struct lnMinList *mixed) { struct vision_track *vt; assert(unknown != NULL); assert(mixed != NULL); vt = (struct vision_track *)mixed->lh_Head; while (vt->vt_link.ln_Succ != NULL) { struct vision_track *vt_succ; vt_succ = (struct vision_track *)vt->vt_link.ln_Succ; if (vt->vt_userdata == NULL) { lnRemove(&vt->vt_link); lnAddTail(unknown, &vt->vt_link); } vt = vt_succ; } }
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; } }
int mypcap_init(pcap_t *pd, pcap_handler callback) { int retval = 0; mypcap_data.mpd_callback = callback; if (pthread_mutex_init(&mypcap_data.mpd_mutex, NULL) != 0) { fprintf(stderr, "error: pthread_mutex_init\n"); } else if (pthread_cond_init(&mypcap_data.mpd_cond, NULL) != 0) { fprintf(stderr, "error: pthread_cond_init\n"); } else if (pthread_create(&mypcap_data.mpd_reader, NULL, mypcap_reader, pd) != 0) { fprintf(stderr, "error: pthread_cond_init\n"); } else { int lpc; lnNewList(&mypcap_data.mpd_buflist); lnNewList(&mypcap_data.mpd_freelist); retval = 1; for (lpc = 0; lpc < MPD_BUFFER_COUNT && retval; lpc++) { if ((mypcap_data.mpd_buffer[lpc] = malloc(pd->bufsize)) == NULL) { retval = 0; } lnAddTail(&mypcap_data.mpd_freelist, &mypcap_data.mpd_nodes[lpc]); } } 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; }