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; }
void vtCoalesce(struct lnMinList *extra, struct lnMinList *now, struct vmc_client *vc, unsigned int vc_len) { struct vision_track *vt; assert(extra != NULL); assert(now != NULL); assert(vc != NULL); assert(vc_len > 0); /* * Walk the list and munge it along the way. So, as we progress through * the list, any tracks before the current one will be completely coalesced * and any tracks after will be those remaining after previous coalesces. */ vt = (struct vision_track *)now->lh_Head; 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. XXX Should we pad the camera bounds by * COALESCE_TOLERANCE since the cameras might be off by a bit? */ for (lpc = 0; lpc < vc_len; lpc++) { if ((vt->vt_position.x >= vc[lpc].vc_left) && (vt->vt_position.x <= vc[lpc].vc_right) && (vt->vt_position.y >= vc[lpc].vc_top) && (vt->vt_position.y <= vc[lpc].vc_bottom)) { in_camera_count += 1; } } assert(in_camera_count > 0); /* * we want to give a slow move-over to the new camera -- i.e., we * really don't want to see the position bouncing back and forth * between cameras... * * how to do this ... ? * * forget slow for now, let's just try to pick the best one. * - WENT WITH THIS APPROACH FOR NOW!!! * * the problem would be if a robot sits right on the fricking * boundary; if it does, then we could easily get positions that * switch off between cameras every ... * * but wait! we can keep track of orientation for each robot's track; * since we have that, we can choose which camera is the "overlap" * camera, and enforce some small buffering to ensure that the robot * is really moving into the next camera before we allow the switch to * proceed. * */ if (in_camera_count > 1) { struct lnMinList near; int rc; lnNewList(&near); /* * Extract any other tracks that are close to the current one and * in the remaining cameras. */ if ((rc = vtExtractNear(&near, vt, vtNextCamera(vt), COALESCE_TOLERANCE)) > 0) { float weight, total_weight = 0.0; struct vision_track *vt_near; struct robot_position rp; if (debug > 2) { printf("start %.2f %.2f\n", vt->vt_position.x, vt->vt_position.y); } /* Compute the weight for this track, younger is better. */ weight = curvy((float)vt->vt_age / (float)MAX_TRACK_AGE); total_weight += weight; /* Initialize the position data with the current track. */ rp.x = vt->vt_position.x * weight; rp.y = vt->vt_position.y * weight; rp.theta = vt->vt_position.theta; rp.timestamp = vt->vt_position.timestamp; /* Add in the position data from the other tracks. */ vt_near = (struct vision_track *)near.lh_Head; while (vt_near->vt_link.ln_Succ != NULL) { if (debug > 2) { printf("merge %.2f %.2f\n", vt_near->vt_position.x, vt_near->vt_position.y); } weight = curvy((float)vt_near->vt_age / (float)MAX_TRACK_AGE); total_weight += weight; rp.x += vt_near->vt_position.x * weight; rp.y += vt_near->vt_position.y * weight; // rp.theta += vt_near->vt_position.theta; vt->vt_age = min(vt->vt_age, vt_near->vt_age); vt->vt_position.timestamp = max(vt->vt_position.timestamp, vt_near->vt_position.timestamp); vt_near = (struct vision_track *)vt_near->vt_link.ln_Succ; } /* Return merged tracks to the pool. */ lnAppendList(extra, &near); rp.x /= total_weight; rp.y /= total_weight; if (debug > 2) { printf("final %.2f %.2f\n", rp.x, rp.y); } // rp.theta /= rc; vt->vt_position = rp; } } vt = (struct vision_track *)vt->vt_link.ln_Succ; } }