평행주차 matlab - erp 적용

vdims = vehicleDimensions;
vdims.Length = 2.4;
vdims.Width = 1.1;
vdims.Wheelbase = 1.6;

egoWheelbase = vdims.Wheelbase;
distToCenter = 0.5*egoWheelbase;

우리 차량 크기를 입력해줍니다.

vehiclePose = updateVehiclePose(modelStatesSub, tftree);
egoInitialPose = vehiclePose';
v=0;
w=0;

egoTargetPose = [20.759-distToCenter,-3,0];​

그리고 차량 현재 위치를 받아와서 현재 위치를 넣어주고, 목표 위치도 넣어줍니다.

Tv = 0.1;
xlim = [20.759-distToCenter-10 20.759-distToCenter+10];   
ylim = [-3-4 -3+4];     
yawlim = [-3.1416 3.1416]; 
bounds = [xlim;ylim;yawlim];
stateSpace = stateSpaceReedsShepp(bounds);
stateSpace.MinTurningRadius = 7;

목표 지점을 기준으로 크기도 잡아줍니다.

stateValidator = parkingStateValidator(stateSpace);
planner = plannerRRTStar(stateSpace,stateValidator);
planner.MaxConnectionDistance = 4;
planner.ContinueAfterGoalReached = true;
planner.MaxIterations = 2000;

플레너도 만들어주고

rng(9, 'twister');
[pathObj,solnInfo] = plan(planner,egoInitialPose,egoTargetPose);
figure;
hold on;
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'y.-'); % tree expansion

경로 탐색 시작

p = 100;
pathObj.interpolate(p+1);
xRef = pathObj.States;
worldWaypoints = xRef;
plot(xRef(:,1), xRef(:,2),'b-','LineWidth',2)
pp.Waypoints = worldWaypoints(:,1:2);

그 경로중 최적 경로 찾아서 waypoint에 넣어주기

 a = vehiclePose';
    if v >= 0 
        plot(a(1),a(2),'r.', 'MarkerSize', 20);
    else
        plot(a(1),a(2),'g.', 'MarkerSize', 20);
    end

이제 여기서부턴 while문 안에서의 내용
현재 위치를 그래프에 그려줍니다.

그럼 끝

이게 추종 알고리즘을 변경하지 않고 따라갔더니 후진을 안하네요 ㅎㅎ
PP엔 후진이 없다는 것을 까먹었습니다..

.... 후진을 못하네요...........