/**
* 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 vtMatch(struct lnMinList *pool,
struct lnMinList *prev,
struct lnMinList *now)
{
struct vision_track *vt;
assert(pool != NULL);
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) {
float distance = MERGE_TOLERANCE, dummy = MERGE_TOLERANCE;
struct vision_track *vt_prev;
void dump_vision_list(struct lnMinList *list);
/*
* Check both ways, the current track is the close to one in the
* previous frame, and the track from the previous frame is closest to
* the one in the current frame.
*/
if (((vt_prev = vtFindMin(vt,
(struct vision_track *)prev->lh_Head,
&distance)) != NULL) &&
(vtFindMin(vt_prev,
(struct vision_track *)now->lh_Head,
&dummy) == vt)) {
vt->vt_userdata = vt_prev->vt_userdata;
vt->ma = vt_prev->ma;
lnRemove(&vt_prev->vt_link);
lnAddHead(pool, &vt_prev->vt_link);
}
else {
#if 0
info("no match %.2f %.2f\n", vt->vt_position.x, vt->vt_position.y);
dump_vision_list(now);
info("==\n");
dump_vision_list(prev);
#endif
}
vt = (struct vision_track *)vt->vt_link.ln_Succ;
}
}
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 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;
}
}
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;
}
