Commit dd5b7e5e authored by Timothy Stack's avatar Timothy Stack

Fix the goal in obstacle check.

parent 4a4d3391
......@@ -191,8 +191,8 @@ pp_plot_code_t pp_plot_waypoint(struct path_plan *pp)
do {
struct obstacle_node *on, *min_on = NULL;
int goal_in_ob = 0, robot_ob = 0;
float min_distance = FLT_MAX;
int robot_ob = 0;
/*
* The path doesn't cross enough of the obstacle box for us to worry
......@@ -230,6 +230,12 @@ pp_plot_code_t pp_plot_waypoint(struct path_plan *pp)
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 */
......@@ -271,9 +277,7 @@ pp_plot_code_t pp_plot_waypoint(struct path_plan *pp)
&on->on_link);
}
if (rc_compute_code(pp->pp_goal_pos.x,
pp->pp_goal_pos.y,
&pp->pp_obstacle) == 0) {
if (goal_in_ob) {
struct robot_position rp;
struct rc_line rl;
float r;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment