Check vehicle costmap for occupied poses or points
checkOccupied function checks whether vehicle poses or
points are occupied by obstacles on the vehicle costmap. Path planning algorithms use
checkOccupied to check whether candidate vehicle poses along a
path are navigable.
To simplify the collision check for a vehicle pose,
vehicleCostmap inflates obstacles according to the vehicle's
InflationRadius, as specified by the
CollisionChecker property of the costmap. The collision checker
calculates the inflation radius by enclosing the vehicle in a set of overlapping circles
of radius R, where the centers of these circles lie along the
longitudinal axis of the vehicle. The inflation radius is the minimum
R needed to fully enclose the vehicle in these circles. A vehicle
pose is collision-free when none of the centers of these circles lie on an inflated grid
cell. For more details, see the algorithm on
vehicleCostmap reference page.
Check If Sequence of Poses Enters Occupied Cell
Load a costmap from a parking lot.
data = load('parkingLotCostmap.mat'); parkMap = data.parkingLotCostmap; plot(parkMap)
Create vehicle poses following a straight-line path.
y are the (x,y) coordinates of the rear axle of the vehicle.
theta is the angle of the rear axle with respect to the x-axis. Note that the dimensions of the vehicle are stored in the
vehicleDimensions property of the costmap, and that there is an offset between the rear axle of the vehicle and its center.
x = 6:0.25:10; y = repmat(5,size(x)); theta = zeros(size(x)); vehiclePoses = [x',y',theta']; hold on plot(x,y,'b.')
Check if the poses are occupied.
occ = checkOccupied(parkMap,vehiclePoses)
occ = 17x1 logical array 0 0 0 0 0 1 1 1 1 1 ⋮
The vehicle poses are occupied beginning with the sixth pose. In other words, the center of the vehicle in the sixth pose lies within the inflation radius of an occupied grid cell.