Fixed segfault in new MotionPlanner code when environments were empty (small islands). #2511
This commit is contained in:
parent
2f255620c6
commit
79cb350f2d
1 changed files with 16 additions and 3 deletions
|
@ -111,12 +111,21 @@ MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyl
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// get environment
|
||||||
|
ExPolygonCollection env = this->get_env(island_idx);
|
||||||
|
if (env.expolygons.empty()) {
|
||||||
|
// if this environment is empty (probably because it's too small), perform straight move
|
||||||
|
// and avoid running the algorithms on empty dataset
|
||||||
|
polyline->points.push_back(from);
|
||||||
|
polyline->points.push_back(to);
|
||||||
|
return; // bye bye
|
||||||
|
}
|
||||||
|
|
||||||
// Now check whether points are inside the environment.
|
// Now check whether points are inside the environment.
|
||||||
Point inner_from = from;
|
Point inner_from = from;
|
||||||
Point inner_to = to;
|
Point inner_to = to;
|
||||||
bool from_is_inside, to_is_inside;
|
bool from_is_inside, to_is_inside;
|
||||||
|
|
||||||
ExPolygonCollection env = this->get_env(island_idx);
|
|
||||||
if (!(from_is_inside = env.contains(from))) {
|
if (!(from_is_inside = env.contains(from))) {
|
||||||
// Find the closest inner point to start from.
|
// Find the closest inner point to start from.
|
||||||
inner_from = this->nearest_env_point(env, from, to);
|
inner_from = this->nearest_env_point(env, from, to);
|
||||||
|
@ -210,8 +219,12 @@ MotionPlanner::nearest_env_point(const ExPolygonCollection &env, const Point &fr
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// if we're here, return last point (better than nothing)
|
// if we're here, return last point if any (better than nothing)
|
||||||
return pp.front();
|
if (!pp.empty()) return pp.front();
|
||||||
|
|
||||||
|
// if we have no points at all, then we have an empty environment and we
|
||||||
|
// make this method behave as a no-op (we shouldn't get here by the way)
|
||||||
|
return from;
|
||||||
}
|
}
|
||||||
|
|
||||||
MotionPlannerGraph*
|
MotionPlannerGraph*
|
||||||
|
|
Loading…
Add table
Reference in a new issue