MPC-test.zip

classdef HelperPathAnalyzer < matlab.System & matlab.system.mixin.Propagates & ... matlab.system.mixin.SampleTime & matlab.system.mixin.CustomIcon %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' A N-by-3 matrix representing the poses of the % reference path % % 'Directions' A 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' A N-by-1 vector representing the curvature % of the reference path % % 'SpeedProfile' A N-by-1 vector representing the speeds 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 % SpeedProfile - Speeds along the reference path % Directions - Driving directions at each point on the path % Curvatures - Path curvaturesa 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, HelperSpeedProfileGenerator % 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 %SpeedProfile Speed profile along the reference path % SpeedProfile %Directions Driving directions corresponding to RefPoses % Directions %Curvatures Path curvatures % Curvatures end % Public properties (Only used in Simulink) properties(Nontunable, Logical) %HasResetOutput Show Reset output port % Flag indicating if the Reset output port is enabled. % % Default: false HasResetOutput = 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, SpeedProfile, 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 %SpeedProfileInternal SpeedProfileInternal % 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, Logical) %HasReferenceInports Flag indicating if there are refPose, directions % and speedProfile 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 = 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, 'SpeedProfile'); obj.Curvatures = kappas; end %------------------------------------------------------------------ function set.SpeedProfile(obj, speedProfile) validateattributes(speedProfile, {'single', 'double'}, ... {'nonnan', 'real', 'column', 'finite', '>=', 0, 'nonempty'}, ... mfilename, 'SpeedProfile'); obj.SpeedProfile = speedProfile; end %------------------------------------------------------------------ function set.Wheelbase(obj, wheelbase) validateattributes(wheelbase, {'single', 'double'}, ... {'nonnan', 'real', 'scalar', 'finite', 'nonempty