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;
}
}
