Path planning for automated parking using hybrid A* algorithm
19 visualizaciones (últimos 30 días)
Mostrar comentarios más antiguos
SAYANDIP
el 7 de Feb. de 2024
Comentada: SAYANDIP
el 19 de Feb. de 2024
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 comentario
Respuesta aceptada
Kautuk Raj
el 14 de Feb. de 2024
I understand that you are trying to run the "Automated Parking Valet" example in the Automated Driving Toolbox, which utilizes the hybrid A* algorithm for path planning.
The "Automated Parking Valet" example was updated in MATLAB R2023b. In this version, the example plans the global path using the "hybrid A*" algorithm. Prior to R2023b, the example implemented the "rapidly-exploring random tree" (RRT*) algorithm. For more details on the updates, please see the release notes for R2023b: https://www.mathworks.com/help/driving/release-notes.html
The error you are experiencing is due to attempting to run the example in MATLAB R2023a, which does not include the update for the hybrid A* algorithm.
To resolve this issue, you can upgrade to MATLAB R2023b. Alternatively, you can access the "Automated Parking Valet" example that is compatible with R2023a, which uses the RRT* algorithm, at the following link: https://www.mathworks.com/help/releases/R2023a/driving/ug/automated-parking-valet.html
Más respuestas (0)
Ver también
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!