Check validity of planned vehicle path
checks the validity of a planned vehicle path,
isValid = checkPathValidity(
the vehicle costmap. Use this function to test if a path is valid within a changing
A path is valid if the following conditions are true:
The path has at least one pose.
The path is collision-free and within the limits of
Plan Path and Check Its Validity
Plan a vehicle path through a parking lot by using the optimal rapidly exploring random tree (RRT*) algorithm. Check that the path is valid, and then plot the transition poses along the path.
Load a costmap of a parking lot. Plot the costmap to see the parking lot and inflated areas for the vehicle to avoid.
data = load('parkingLotCostmap.mat'); costmap = data.parkingLotCostmap; plot(costmap)
Define start and goal poses for the vehicle as [x, y, Θ] vectors. World units for the (x,y) locations are in meters. World units for the Θ orientation angles are in degrees.
startPose = [4, 4, 90]; % [meters, meters, degrees] goalPose = [30, 13, 0];
pathPlannerRRT object to plan a path from the start pose to the goal pose.
planner = pathPlannerRRT(costmap); refPath = plan(planner,startPose,goalPose);
Check that the path is valid.
isPathValid = checkPathValidity(refPath,costmap)
isPathValid = logical 1
Interpolate the transition poses along the path.
transitionPoses = interpolate(refPath);
Plot the planned path and the transition poses on the costmap.
hold on plot(refPath,'DisplayName','Planned Path') scatter(transitionPoses(:,1),transitionPoses(:,2),,'filled', ... 'DisplayName','Transition Poses') hold off
refPath — Planned vehicle path
Planned vehicle path, specified as a
costmap — Costmap used for collision checking
Costmap used for collision checking, specified as a
refPoses — Sequence of vehicle poses
m-by-3 matrix of [x,
y, Θ] vectors
Sequence of vehicle poses, specified as an m-by-3 matrix of [x, y, Θ] vectors. m is the number of specified poses.
x and y specify the location of the
vehicle. These values must be in the same world units used by
Θ specifies the orientation angle of the vehicle in degrees.
isValid — Indicates validity of path or poses
A path or sequence of poses is valid (
1) if the
following conditions are true:
The path or pose sequence has at least one pose.
The path or pose sequence is collision-free and within the limits of
To check if a vehicle path is valid, the
discretizes the path. Then, the function checks that the poses at the discretized points
are collision-free. The threshold for a collision-free pose depends on the resolution at