Path planning for automated parking using hybrid A* algorithm
Show older comments
Whenever I am performing the code for automated parking using A* , for the HelperPathAnalyzer.m file i am getting this error.
Unrecognized property 'PlanningPeriod' for class 'HelperPathAnalyzer'.
% ENVIRONMENT MODEL
% Load and display the three map layers. In each layer, dark cells
% represent occupied cells, and light cells represent free cells.
mapLayers = loadParkingLotMapLayers;
plotMapLayers(mapLayers)
% For simplicity, combine the three layers into a single costmap.
costmap = combineMapLayers(mapLayers);
figure
plot(costmap, 'Inflation', 'off')
legend off
% The |costmap| covers the entire 75m-by-50m parking lot area, divided into
% 0.5m-by-0.5m square cells.
costmap.MapExtent % [x, width, y, height] in meters
costmap.CellSize % cell size in meters
% Create a object for
% storing the dimensions of the vehicle that will park automatically. Also
% define the maximum steering angle of the vehicle. This value determines
% the limits on the turning radius during motion planning and control.
vehicleDims = vehicleDimensions;
maxSteeringAngle = 45; % in degrees
% Update the |VehicleDimensions| property of the costmap collision checker
% with the dimensions of the vehicle to park. This setting adjusts the
% extent of the inflation in the map around obstacles to correspond to the
% size of the vehicle being parked, ensuring that collision-free paths can
% be found through the parking lot.
costmap.CollisionChecker.VehicleDimensions = vehicleDims;
% Define the starting pose of the vehicle. The pose is obtained through
% localization, which is left out of this example for simplicity. The
% vehicle pose is specified as $[x,y,\theta]$, in world coordinates.
% $(x,y)$ represents the position of the center of the vehicle's rear axle
% in world coordinate system. $\theta$ represents the orientation of the
% vehicle with respect to world X axis. For more details, see
% <docid:driving_ug#bvgu_ya-2 Coordinate Systems in Automated Driving Toolbox>.
currentPose = [4 12 0]; % [x, y, theta]
% BEHAVORIAL LAYER
data = load('routePlan.mat');
routePlan = data.routePlan
% Plot vehicle at current pose
hold on
helperPlotVehicle(currentPose, vehicleDims, DisplayName="Current Pose")
legend(Location="northwest")
for n = 1 : height(routePlan)
% Extract the goal waypoint
vehiclePose = routePlan{n, "EndPose"};
% Plot the pose
legendEntry = "Goal " + n;
helperPlotVehicle(vehiclePose, vehicleDims, DisplayName=legendEntry);
end
hold off
% Create the behavioral planner helper object. The |requestManeuver| method
% requests a stream of navigation tasks from the behavioral planner until
% the destination is reached.
behavioralPlanner = HelperBehavioralPlanner(routePlan, maxSteeringAngle);
% MOTION PLANNER --> Hybrid A*
% Create a pathPlannerRRT>| object to configure a path planner using an optimal
% rapidly exploring random tree (Hybrid A*) approach. The RRT family of planning
% algorithms find a path by constructing a tree of connected,
% collision-free vehicle poses. Poses are connected using Dubins or
% Reeds-Shepp steering, ensuring that the generated path is kinematically
% feasible.
ss = stateSpaceSE2;
ss.StateBounds = [costmap.MapExtent(1, 1:2); costmap.MapExtent(1, 3:4); -pi, pi];
validator = validatorVehicleCostmap(ss, Map=costmap);
minTurningRadius = 10; % in meters
motionPlanner = plannerHybridAStar(validator, MinTurningRadius=minTurningRadius, ...
MotionPrimitiveLength=4); % length in meters
% Plan a path from the current pose to the first goal by using the |plan|
% function. The returned driving.Path| object, |refPath|, is a feasible and collision-free
% reference path.
goalPose = routePlan{1, "EndPose"};
currentPoseRad = [currentPose(1:2) deg2rad(currentPose(3))];
goalPoseRad = [goalPose(1:2) deg2rad(goalPose(3))];
refPath = plan(motionPlanner, currentPoseRad, goalPoseRad);
plannerAxes = show(motionPlanner, Tree="off", HeadingLength=0); % Visualize the planned path.
% The reference path consists of a sequence of path segments. Each path
% segment describes the set of Dubins or Reeds-Shepp maneuvers used to
% connect to the next segment. Inspect the path segments.
weights = struct(Time=10, Smoothness=100, Obstacle=50);
vehicleInfo = struct("Dimension", [vehicleDims.Length, vehicleDims.Width], "Shape", "Rectangle");
MinTurningRadius=minTurningRadius
localPlanner = controllerTEB(zeros(3,3),...
MaxVelocity=[5 0.5],... % in m/s and rad/s
MaxAcceleration=[2 0.5],... % in m/s/s and rad/s/s
LookAheadTime=3,... % in seconds
NumIteration=2,...
CostWeights=weights, RobotInformation=vehicleInfo);
% The local planner does not require knowledge of the full parking lot map
% since it is only planning a short local trajectory. Providing the local
% planner with a smaller map increases planner performance.
maxLocalPlanDistance = 15; % in meters
localPlanner.Map = getLocalMap(costmap, currentPose, maxLocalPlanDistance);
localPlanner.ReferencePath = refPath;
% Along with an optimized trajectory, |localPath|, the local planner provides
% a reference velocity for each point on the path, |refVel|.
[refVel, ~, localPath, ~] = localPlanner(currentPoseRad, [0 0]);
% Show the local trajectory alongside the reference path.
hold(plannerAxes,"on");
plot(plannerAxes, localPath(:,1), localPath(:,2), "g", LineWidth=2, DisplayName="Local Path");
legend(plannerAxes, "show", findobj(plannerAxes, Type="Line"), {"Local Path","ReferencePath"});
Vehicle control and Simulation
closeFigures; % Close all
% Create the vehicle simulator.
vehicleSim = HelperVehicleSimulator(costmap, vehicleDims);
hideFigure(vehicleSim); % Hide vehicle simulation figure
% Configure the simulator to show the trajectory.
vehicleSim.showTrajectory(true);
% vehicleSim.showLegend(true);
% Set the vehicle pose and velocity.
vehicleSim.setVehiclePose(currentPose);
currentVel = 0;
vehicleSim.setVehicleVelocity(currentVel);
pathAnalyzer = HelperPathAnalyzer(Wheelbase=vehicleDims.Wheelbase);
sampleTime = 0.05;
lonController = HelperLongitudinalController(SampleTime=sampleTime);
controlRate = HelperFixedRate(1/sampleTime); % in Hertz
% Set the vehicle pose back to the initial starting point.
currentPose = [4 12 0]; % [x, y, theta]
vehicleSim.setVehiclePose(currentPose);
% Reset velocity.
currentVel = 0; % meters/second
vehicleSim.setVehicleVelocity(currentVel);
% Initialize variables to store vehicle path.
refPath = [];
localPath = [];
% Setup pathAnalyzer to trigger update of local path every 1 second.
localPlanningFrequency = 1; % 1/seconds
pathAnalyzer.PlanningPeriod = 1/localPlanningFrequency/sampleTime; % timesteps // giving the above error.
isGoalReached = false;
% Initialize count incremented each time the local planner is updated
localPlanCount = 0; % Used for visualization only
% showFigure(vehicleSim); % Show vehicle simulation figure.
while ~isGoalReached
% Plan for the next path segment if near to the next path segment start
% pose.
if planNextSegment(behavioralPlanner, currentPose, 2*maxLocalPlanDistance) % // Unrecognized function or variable 'planNextSegment'. // Another error
% Request next maneuver from behavioral layer.
[nextGoal, plannerConfig, speedConfig] = requestManeuver(behavioralPlanner, ...
currentPose, currentVel);
% Plan a reference path using A* planner to the next goal pose.
if isempty(refPath)
nextStartRad = [currentPose(1:2) deg2rad(currentPose(3))];
else
nextStartRad = refPath(end,:);
end
nextGoalRad = [nextGoal(1:2) deg2rad(nextGoal(3))];
newPath = plan(motionPlanner, nextStartRad, nextGoalRad, SearchMode="exhaustive");
% Check if the path is valid. If the planner fails to compute a path,
% or the path is not collision-free because of updates to the map, the
% system needs to re-plan. This scenario uses a static map, so the path
% will always be collision-free.
isReplanNeeded = ~checkPathValidity(newPath.States, costmap);
if isReplanNeeded
warning("Unable to find a valid path. Attempting to re-plan.")
% Request behavioral planner to re-plan
replanNeeded(behavioralPlanner);
else
% Append to refPath
refPath = [refPath; newPath.States];
hasNewPath = true;
vehicleSim.plotReferencePath(refPath); % Plot reference path
end
end
% Update the local path at the frequency specified by
% |localPlanningFrequency|
if pathUpdateNeeded(pathAnalyzer)
currentPose = getVehiclePose(vehicleSim);
currentPoseRad = [currentPose(1:2) deg2rad(currentPose(3))];
currentVel = getVehicleVelocity(vehicleSim);
currentAngVel = getVehicleAngularVelocity(vehicleSim);
% Do local planning
localPlanner.Map = getLocalMap(costmap, currentPose, maxLocalPlanDistance);
if hasNewPath
localPlanner.ReferencePath = refPath;
hasNewPath = false;
end
[localVel, ~, localPath, ~] = localPlanner(currentPoseRad, [currentVel currentAngVel]);
vehicleSim.plotLocalPath(localPath); % Plot new local path
% For visualization only
if mod(localPlanCount, 20) == 0
snapnow; % Capture state of the figures
end
localPlanCount = localPlanCount+1;
% Configure path analyzer.
pathAnalyzer.RefPoses = [localPath(:,1:2) rad2deg(localPath(:,3))];
pathAnalyzer.Directions = sign(localVel(:,1));
pathAnalyzer.VelocityProfile = localVel(:,1);
end
% Find the reference pose on the path and the corresponding
% velocity.
[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);
% Update driving direction for the simulator.
updateDrivingDirection(vehicleSim, direction);
% Compute steering command.
steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
Direction=direction, Wheelbase=vehicleDims.Wheelbase, PositionGain=4);
% Compute acceleration and deceleration commands.
lonController.Direction = direction;
[accelCmd, decelCmd] = lonController(refVel, currentVel);
% Simulate the vehicle using the controller outputs.
drive(vehicleSim, accelCmd, decelCmd, steeringAngle);
% Get current pose and velocity of the vehicle.
currentPose = getVehiclePose(vehicleSim);
currentVel = getVehicleVelocity(vehicleSim);
% Check if the vehicle reaches the goal.
isGoalReached = helperGoalChecker(nextGoal, currentPose, currentVel, speedConfig.EndSpeed, direction);
% Wait for fixed-rate execution.
waitfor(controlRate);
end
In the HelperPathAnalyzer.m file there is no such property called PlanningPeriod .
classdef HelperPathAnalyzer < matlab.System
%HelperPathAnalyzer Provide reference inputs for vehicle controllers.
% HelperPathAnalyzer computes the reference pose and the reference
% velocity based on the current pose of the vehicle.
%
% pathAnalyzer = HelperPathAnalyzer creates a system object,
% pathAnalyzer, that calculate reference inputs for vehicle controllers.
%
% pathAnalyzer = HelperPathAnalyzer(Name,Value) creates a system object,
% pathAnalyzer, with additional options specified by one or more
% Name,Value pair arguments:
%
% 'Wheelbase' Wheelbase of the vehicle
%
% Default: 2.8 (meters)
%
% 'RefPoses' An N-by-3 matrix representing the poses of the
% reference path
%
% 'Directions' An N-by-1 vector representing the driving
% directions at each point on the reference path.
% The vector is composed by possible values: 1
% for forward motion and -1 for reverse motion.
%
% 'Curvatures' An N-by-1 vector representing the curvature
% of the reference path
%
% 'VelocityProfile' An N-by-1 vector representing the velocities along
% the reference path (in meters/second)
%
%
% Step method syntax:
% [refPose, refVel, direction] = step(pathAnalyzer, currPose, currVel)
% returns the reference pose, refPose, reference velocity, refVel, and
% the driving direction based on the current pose, currPose, and the
% current velocity, currVel, of the vehicle.
%
% System objects may be called directly like a function instead of using
% the step method. For example, y = step(obj) and y = obj() are equivalent.
%
% HelperPathAnalyzer properties:
% Wheelbase - Wheelbase of the vehicle
% RefPoses - Poses of the reference path
% VelocityProfile - Velocities along the reference path
% Directions - Driving directions at each point on the path
% Curvatures - Path curvatures at each point on the path
%
% HelperPathAnalyzer methods:
% step - Compute reference poses, velocity, and direction
% release - Allow property value changes
% clone - Create a copy of the object
% isLocked - Locked status (logical)
%
% See also lateralControllerStanley, HelperLongitudinalController,
% smoothPathSpline, helperGenerateVelocityProfile
% Copyright 2018 The MathWorks, Inc.
% Public, non-tunable properties
properties(Nontunable)
%Wheelbase Vehicle wheelbase (m)
% A scalar specifying the distance between the front and the rear
% axles.
%
% Default: 2.8 (m)
Wheelbase = 2.8
end
% Public properties (Only used in MATLAB)
properties
%RefPoses Vehicle poses along the reference path
%
RefPoses
%VelocityProfile Speed profile along the reference path
%
VelocityProfile
%Directions Driving directions corresponding to RefPoses
%
Directions
%Curvatures Path curvatures
%
Curvatures
end
% Public properties (Only used in Simulink)
properties(Nontunable)
%HasResetOutput Show Reset output port
% Flag indicating if the Reset output port is enabled.
%
% Default: false
HasResetOutput (1, 1) logical = false;
end
properties(Access = private)
%ClosestPointIndex Store the previous projection point index
% to handle encircled path
%
% Default: 1
ClosestPointIndex = 1
%NumPathSegments Number of segments in a path. When
%
% Default: 1
NumPathSegments = 1
%CurrentSegmentIndex Index of the current segment
%
CurrentSegmentIndex = 1
%SegmentStartIndex A vector storing the indices of the starting
% points of all the path segments
%
SegmentStartIndex
%SegmentStartIndex A vector storing the indices of the ending
% points of all the path segments
SegmentEndIndex
% The following four properties are used to transfer reference
% data within the system object. Depending on the environment the
% object is executing in, they are assigned either by public
% properties, RefPoses, VelocityProfile, Directions and Curvatures
% in MATLAB, or by the input ports in Simulink. The selection is
% determined by the HasReferenceInports property.
%RefPosesInternal
RefPosesInternal
%DirectionsInternal
DirectionsInternal
%CurvaturesInternal
CurvaturesInternal
%VelocityProfileInternal
VelocityProfileInternal
% The following four properties are used to store the last output.
%LastRefPoseOutput
LastRefPoseOutput = [0 0 0]
%LastRefVelocityOutput
LastRefVelocityOutput = 0
%LastCurvatureOutput
LastCurvatureOutput = 0
%LastDirectionOutput
LastDirectionOutput = 1
end
properties(Access = private, Nontunable)
%HasReferenceInports Flag indicating if there are refPose, directions,
% and VelocityProfile inputs in stepImp. In MATLAB, all these
% values are set via properties while in Simulink they are
% passed as inputs via input ports.
%
% Default: false
HasReferenceInports (1, 1) logical = false
end
%----------------------------------------------------------------------
% Setter and constructor
%----------------------------------------------------------------------
methods
%------------------------------------------------------------------
function set.RefPoses(obj, refPoses)
validateattributes(refPoses, {'single', 'double'}, ...
{'nonnan', 'real', 'ncols', 3, 'finite', 'nonempty'}, ...
mfilename, 'RefPoses');
obj.RefPoses = refPoses;
end
%------------------------------------------------------------------
function set.Directions(obj, directions)
validateattributes(directions, {'single', 'double'}, ...
{'nonnan', 'real', 'column', 'finite', 'nonempty'}, ...
mfilename, 'Directions');
obj.Directions = directions;
end
%------------------------------------------------------------------
function set.Curvatures(obj, kappas)
validateattributes(kappas, {'single', 'double'}, ...
{'nonnan', 'real', 'column', 'finite', 'nonempty'}, ...
mfilename, 'Curvatures');
obj.Curvatures = kappas;
end
%------------------------------------------------------------------
function set.VelocityProfile(obj, VelocityProfile)
validateattributes(VelocityProfile, {'single', 'double'}, ...
{'nonnan', 'real', 'column', 'finite', 'nonempty'}, ...
mfilename, 'VelocityProfile');
obj.VelocityProfile = VelocityProfile;
end
%------------------------------------------------------------------
function set.Wheelbase(obj, wheelbase)
validateattributes(wheelbase, {'single', 'double'}, ...
{'nonnan', 'real', 'scalar', 'finite', 'nonempty'}, ...
mfilename, 'Wheelbase');
obj.Wheelbase = wheelbase;
end
%------------------------------------------------------------------
function obj = HelperPathAnalyzer(varargin)
%HelperPathAnalyzer Constructor
setProperties(obj,nargin,varargin{:}, 'RefPoses', ...
'VelocityProfile','Directions', 'Wheelbase');
end
end
%----------------------------------------------------------------------
% Main algorithm
%----------------------------------------------------------------------
methods(Access = protected)
%------------------------------------------------------------------
function setupImpl(obj, ~, ~, varargin)
%setupImpl Perform one-time calculations
obj.ClosestPointIndex = 1;
obj.NumPathSegments = 1;
obj.CurrentSegmentIndex = 1;
if isSimulinkBlock(obj) % In Simulink
obj.HasReferenceInports = true;
obj.RefPosesInternal = nan(size(varargin{1}), 'like', varargin{1});
obj.DirectionsInternal = nan(size(varargin{2}), 'like', varargin{2});
obj.CurvaturesInternal = nan(size(varargin{3}), 'like', varargin{3});
obj.VelocityProfileInternal = nan(size(varargin{4}), 'like', varargin{4});
else % In MATLAB
obj.HasReferenceInports = false;
end
end
%------------------------------------------------------------------
function processTunedPropertiesImpl(obj)
% processTunedPropertiesImpl Perform actions when tunable
% properties change between calls to the System object
propChange = isChangedProperty(obj,'RefPoses') || ...
isChangedProperty(obj,'VelocityProfile') || ...
isChangedProperty(obj,'Directions');
if propChange
obj.CurrentSegmentIndex = 1;
obj.ClosestPointIndex = 1;
end
end
%------------------------------------------------------------------
function [refPose, refVel, direction, curvature, varargout] = stepImpl(obj, ...
currPose, currVel, varargin)
%stepImpl Implement the main algorithm and return the reference
% pose, velocity and driving direction. varargout is an
% optional output in Simulink that signifies reaching
% intermediate goals within a reference path, i.e., reaching
% the direction-switching positions.
if obj.HasReferenceInports
% Check if the reference path is new
if ~isequal(obj.RefPosesInternal, varargin{1})
obj.RefPosesInternal = varargin{1};
obj.DirectionsInternal = varargin{2};
obj.CurvaturesInternal = varargin{3};
obj.VelocityProfileInternal = varargin{4};
obj.CurrentSegmentIndex = 1;
obj.ClosestPointIndex = 1;
end
else % In MATLAB, values are from properties
obj.RefPosesInternal = obj.RefPoses;
obj.DirectionsInternal = obj.Directions;
obj.VelocityProfileInternal = obj.VelocityProfile;
obj.CurvaturesInternal = obj.Curvatures;
end
% Divide the path to segments based on driving direction
findSegmentBoundaryPointIndex(obj);
% Check if reaching the final goal. If yes, use the previous
% outputs
if obj.CurrentSegmentIndex > obj.NumPathSegments
refPose = obj.LastRefPoseOutput;
refVel = obj.LastRefVelocityOutput;
direction = obj.LastDirectionOutput;
curvature = obj.LastCurvatureOutput;
if obj.HasResetOutput && isSimulinkBlock(obj)
varargout{1} = 1;
end
return
end
% Get the desired pose, desired velocity and driving direction
[refPose, refVel, direction, curvature] = findDesiredPoseAndVelocity(obj, currPose);
% Check if the vehicle reaches the intermediate goal. If yes,
% increment the path segment index and reset reference velocity
% to zero as the vehicle needs to switch direction at the
% intermediate goal positions
currGoalIndex = obj.SegmentEndIndex(obj.CurrentSegmentIndex);
nextGoal = obj.RefPosesInternal(currGoalIndex, :);
endRefVel = obj.VelocityProfileInternal(currGoalIndex, :);
reset = 0;
if helperGoalChecker(nextGoal, currPose, currVel, endRefVel, direction)
obj.CurrentSegmentIndex = obj.CurrentSegmentIndex + 1;
refVel = endRefVel;
reset = 1;
end
if obj.HasResetOutput && isSimulinkBlock(obj)
varargout{1} = reset;
end
% Store the output
obj.LastRefPoseOutput = refPose;
obj.LastRefVelocityOutput = refVel;
obj.LastDirectionOutput = direction;
obj.LastCurvatureOutput = curvature;
end
%------------------------------------------------------------------
function [refPose, refVel, direction, curvature] = findDesiredPoseAndVelocity(obj, pose)
%findDesiredPoseAndVelocity Determine the desired pose and
% velocity based on the current pose. The desired pose is
% determined by searching the closest point on the reference
% path. The desired velocity is the velocity corresponding to
% the closest point.
% Get the current segment indexes
segStartIndex = obj.SegmentStartIndex(obj.CurrentSegmentIndex);
segEndIndex = obj.SegmentEndIndex(obj.CurrentSegmentIndex);
pose(3) = deg2rad(pose(3));
% Only search within the current segment of the path
segRefPoses = obj.RefPosesInternal(segStartIndex:segEndIndex, :);
segRefPoses(:, 3) = deg2rad(segRefPoses(:, 3));
% Current driving direction
direction = obj.DirectionsInternal(segEndIndex);
% Compute the index of the closest point on the path segment
segClosestPointIndex = findClosestPathPoint(obj, pose, segRefPoses);
% Convert the segment index to the whole path index
obj.ClosestPointIndex = segClosestPointIndex + segStartIndex - 1;
% Get the desired velocity. Set a lower threshold to avoid zero
% reference velocity at the very beginning.
lowSpeed = 0.1;
if segClosestPointIndex == 1
refVel= max(abs(obj.VelocityProfileInternal(obj.ClosestPointIndex)), lowSpeed)*direction;
else
refVel= obj.VelocityProfileInternal(obj.ClosestPointIndex);
end
% Get the desired pose. In forward motion, the refPose is
% specified for the front wheel.
if direction == 1 % forward
refPose = driving.internal.control.rearPoseToFrontPose(segRefPoses(segClosestPointIndex, 1:3), obj.Wheelbase);
else
refPose = segRefPoses(segClosestPointIndex, 1:3);
end
% Workaround to support lateralControllerStanley in MATLAB
% that does not require curvature input
if ~isempty(obj.CurvaturesInternal)
curvature = obj.CurvaturesInternal(obj.ClosestPointIndex);
else
curvature = 0;
end
refPose(3) = rad2deg(refPose(3));
end
%------------------------------------------------------------------
function closestIdx = findClosestPathPoint(obj, pose, refPoses)
%findClosestPathPoint Find the index of the closest point
segStartIndex = obj.SegmentStartIndex(obj.CurrentSegmentIndex);
if obj.DirectionsInternal(segStartIndex) == 1 % forward driving uses front wheel as reference
pose = driving.internal.control.rearPoseToFrontPose(pose, obj.Wheelbase);
refPoses = driving.internal.control.rearPoseToFrontPose(refPoses, obj.Wheelbase);
end
dis2PointsSquare = (refPoses(:,1)- pose(1)).^2 + (refPoses(:,2)- pose(2)).^2;
% Find the closest point on the reference path
[closestIdx, ~] = find(dis2PointsSquare == min(dis2PointsSquare), 1);
% Enforce to be a scalar in Simulink
closestIdx = closestIdx(1);
% If the reference pose is lagging behind the current pose,
% move to the next reference path.
if obj.moveToNext(refPoses(closestIdx, :), pose) && closestIdx~= size(refPoses, 1)
closestIdx = closestIdx+1;
end
end
%------------------------------------------------------------------
function nextFlag = moveToNext(obj, refPose, pose)
%moveToNext Check if the refPose is lagging behind the current
% pose. If yes, move to the next refPose.
% The is necessary when the vehicle is at accelerating stage.
% When the reference speed is small it takes relatively
% longer time to reach the desired maximum speed. When the
% vehicle reaches somewhere between two reference points,
% use the next one as the reference to set a larger
% reference speed.
wheelbase = obj.Wheelbase;
if obj.DirectionsInternal(1) == 1
poseF = driving.internal.control.rearPoseToFrontPose(pose, wheelbase);
refPoseF = driving.internal.control.rearPoseToFrontPose(refPose, wheelbase);
vec1 = [cos(refPoseF(3)), sin(refPoseF(3))];
vec2 = [poseF(1)-refPoseF(1), poseF(2)-refPoseF(2)];
else
vec1 = [cos(refPose(3)), sin(refPose(3))];
vec2 = [pose(1)-refPose(1), pose(2)-refPose(2)];
end
nextFlag = (vec1*vec2'*obj.DirectionsInternal(1) > 0);
end
end
%----------------------------------------------------------------------
% Common methods
%----------------------------------------------------------------------
methods(Access = protected)
%------------------------------------------------------------------
function validateInputsImpl(obj, currPose, currVelocity, varargin)
% Validate inputs to the step method at initialization
matlabshared.planning.internal.validation.checkPose(currPose, 3, 'currPose', mfilename);
validateattributes(currVelocity, {'double', 'single'}, {'nonnan', ...
'real', 'finite', 'scalar', 'nonempty'}, mfilename, 'currVelocity');
if obj.HasReferenceInports && isSimulinkBlock(obj)
% RefPoses
validateattributes(varargin{1}, {'single', 'double'}, ...
{'nonnan', 'real', 'ncols', 3, 'finite', 'nonempty'}, ...
mfilename, 'refPoses');
% Directions
validateattributes(varargin{2}, {'single', 'double'}, ...
{'nonnan', 'real', 'column', 'finite', 'nonempty'}, ...
mfilename, 'directions');
% Curvatures
validateattributes(varargin{3}, {'single', 'double'}, ...
{'nonnan', 'real', 'column', 'finite', 'nonempty'}, ...
mfilename, 'curvatures');
% VelocityProfile
validateattributes(varargin{4}, {'single', 'double'}, ...
{'nonnan', 'real', 'column', 'finite', 'nonempty'}, ...
mfilename, 'VelocityProfile');
end
end
%------------------------------------------------------------------
function num = getNumInputsImpl(obj)
% Define total number of inputs for system with optional inputs
if isSimulinkBlock(obj)
num = 6;
else
num = 2;
end
end
%------------------------------------------------------------------
function num = getNumOutputsImpl(obj)
% Define total number of outputs for system with optional
% outputs
if ~obj.HasResetOutput
num = 4;
else
num = 5;
end
end
%------------------------------------------------------------------
function s = saveObjectImpl(obj)
% Set properties in structure s to values in object obj
% Set public properties and states
s = saveObjectImpl@matlab.System(obj);
% Set private and protected properties
s.RefPoses = obj.RefPoses;
s.VelocityProfile = obj.VelocityProfile;
s.Directions = obj.Directions;
s.ClosestPointIndex = obj.ClosestPointIndex;
s.NumPathSegments = obj.NumPathSegments;
s.CurrentSegmentIndex = obj.CurrentSegmentIndex;
s.SegmentStartIndex = obj.SegmentStartIndex;
s.SegmentEndIndex = obj.SegmentEndIndex;
s.RefPosesInternal = obj.RefPosesInternal;
s.DirectionsInternal = obj.DirectionsInternal;
s.CurvaturesInternal = obj.CurvaturesInternal;
s.VelocityProfileInternal = obj.VelocityProfileInternal;
s.LastRefPoseOutput = obj.LastRefPoseOutput;
s.LastRefVelocityOutput = obj.LastRefVelocityOutput;
s.LastDirectionOutput = obj.LastDirectionOutput;
end
%------------------------------------------------------------------
function loadObjectImpl(obj,s,wasLocked)
% Set properties in object obj to values in structure s
% Set private and protected properties
obj.RefPoses = s.RefPoses;
obj.VelocityProfile = s.VelocityProfile;
obj.Directions = s.Directions;
obj.ClosestPointIndex = s.ClosestPointIndex;
obj.NumPathSegments = s.NumPathSegments;
obj.CurrentSegmentIndex = s.CurrentSegmentIndex;
obj.SegmentStartIndex = s.SegmentStartIndex;
obj.SegmentEndIndex = s.SegmentEndIndex;
obj.RefPosesInternal = s.RefPosesInternal;
obj.DirectionsInternal = s.DirectionsInternal;
obj.CurvaturesInternal = s.CurvaturesInternal;
obj.VelocityProfileInternal = s.VelocityProfileInternal;
obj.LastRefPoseOutput = s.LastRefPoseOutput;
obj.LastRefVelocityOutput = s.LastRefVelocityOutput;
obj.LastDirectionOutput = s.LastDirectionOutput;
% Set public properties and states
loadObjectImpl@matlab.System(obj,s,wasLocked);
end
end
%----------------------------------------------------------------------
% Simulink-only methods
%----------------------------------------------------------------------
methods(Access = protected)
%------------------------------------------------------------------
function icon = getIconImpl(~)
% Define icon for System block
icon = ["Helper", "Path", "Analyzer"]; % Use class name
end
%------------------------------------------------------------------
function [name1,name2,name3,name4,name5,name6] = getInputNamesImpl(obj)
% Return input port names for System block
name1 = 'CurrPose';
name2 = 'CurrVelocity';
name3 = 'RefPoses';
name4 = 'Directions';
name5 = 'Curvatures';
name6 = 'RefVelocities';
end
%------------------------------------------------------------------
function varargout = getOutputNamesImpl(obj)
% Return output port names for System block
varargout{1} = 'RefPose';
varargout{2} = 'RefVelocity';
varargout{3} = 'Direction';
varargout{4} = 'Curvature';
if obj.HasResetOutput
varargout{5} = 'Reset';
end
end
%------------------------------------------------------------------
function varargout = getOutputSizeImpl(obj)
% Return size for each output port
varargout{1} = [1 3]; % RefPose
varargout{2} = [1 1]; % RefVelocity
varargout{3} = [1 1]; % Direction
varargout{4} = [1 1]; % Curvature
if obj.HasResetOutput
varargout{5} = [1 1]; % Reset
end
end
%------------------------------------------------------------------
function varargout = getOutputDataTypeImpl(obj)
% Return data type for each output port
varargout{1} = propagatedInputDataType(obj,1);
varargout{2} = propagatedInputDataType(obj,1);
varargout{3} = propagatedInputDataType(obj,1);
varargout{4} = propagatedInputDataType(obj,1);
if obj.HasResetOutput
varargout{5} = propagatedInputDataType(obj,1);
end
end
%------------------------------------------------------------------
function varargout = isOutputComplexImpl(obj)
% Return true for each output port with complex data
varargout{1} = false;
varargout{2} = false;
varargout{3} = false;
varargout{4} = false;
if obj.HasResetOutput
varargout{5} = false;
end
end
%------------------------------------------------------------------
function varargout = isOutputFixedSizeImpl(obj)
% Return true for each output port with fixed size
varargout{1} = true;
varargout{2} = true;
varargout{3} = true;
varargout{4} = true;
if obj.HasResetOutput
varargout{5} = true;
end
end
%------------------------------------------------------------------
function flag = isInputSizeMutableImpl(~,index)
% Return false if input size cannot change
% between calls to the System object
% refPoses, directions, curvatures, and speeProfile are variable-size
if any(index == [3,4,5,6])
flag = true;
else
flag = false;
end
end
%------------------------------------------------------------------
function flag = isInactivePropertyImpl(obj,prop)
% Return false if property is visible based on object
% configuration, for the command line and System block dialog
if strcmp(prop, 'HasResetOutput')
flag = ~isSimulinkBlock(obj);
else
flag = false;
end
end
end
%----------------------------------------------------------------------
% Simulink dialog
%----------------------------------------------------------------------
methods(Access = protected, Static)
function group = getPropertyGroupsImpl
% Define property section(s) for System block dialog
group = matlab.system.display.Section(...
'Title', 'Parameters', ...
'PropertyList', {'Wheelbase', 'HasResetOutput'});
end
end
%----------------------------------------------------------------------
% Utility functions
%----------------------------------------------------------------------
methods (Access = protected)
%------------------------------------------------------------------
function flag = isSimulinkBlock(obj)
%isSimulinkBlock Check if the system object in used in Simulink
flag = getExecPlatformIndex(obj); % 0 for MATLAB, 1 for Simulink
end
%------------------------------------------------------------------
function findSegmentBoundaryPointIndex(obj)
%findSegmentBoundaryPointIndex Divide the path to segments
%based on driving direction
directions = obj.DirectionsInternal;
% Find the switching points
switchIndex = find(directions(1:end-1)+directions(2:end)==0);
% Divide the path into segments
obj.SegmentStartIndex = [1; switchIndex+1];
obj.SegmentEndIndex = [switchIndex; length(directions)];
obj.NumPathSegments = numel(obj.SegmentStartIndex);
end
end
end
How to tackle this issue?
1 Comment
Accepted Answer
More Answers (0)
Categories
Find more on Planning and Control in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!