자율주행 자동차 erp-42 gazebo simulation 상 automatic parking 구현, matlab 코드 첨부

 

clear; close all; clc;
rosshutdown;
rosinit('http://localhost:11311')
tftree = rostf;
pause(3);

% Parameter//============================================================
roi = [0, 20, -10, 10, -2, 4];


% init//==================================================================

params = lidarParameters('OS1Gen1-64',512);

velSub = rossubscriber('/odom_skid', "DataFormat", "struct");
gpsSub = rossubscriber('/odometry/gps', "DataFormat", "struct");
imuSub = rossubscriber('/imu/data', "DataFormat", "struct");

lidarSub = rossubscriber('/lidar/points', "DataFormat", "struct");

[pubClusters, markerArrayMsg] = rospublisher('/clusters_marker', 'visualization_msgs/MarkerArray',DataFormat='struct');
pubPath = rospublisher('/path_marker', 'visualization_msgs/Marker','DataFormat','struct');

detectParkingSpace = true;

pp=controllerPurePursuit;
pp.LookaheadDistance=4; % m
pp.DesiredLinearVelocity=1.3; % m/s
pp.MaxAngularVelocity = 1.5; % rad/s
x = 0:0.5:80;
y = zeros(size(x));
z = [x;y]';
pp.Waypoints = z;
while detectParkingSpace 
    lidarData = receive(lidarSub);
    
    
    roiPtCloud = preprocess_lidar_data(lidarData, params, roi);
    
    roiPtCloud = pcdenoise(roiPtCloud,PreserveStructure=true,NumNeighbors=30);
       
    % 포인트클라우드 N 이상일때만 물체로 판단
    [labels, numClusters] = pcsegdist(roiPtCloud,0.3,"NumClusterPoints",15);
    
    vehiclePose = getPose(gpsSub, imuSub);
    if numClusters > 1
        pp.DesiredLinearVelocity=0.5; % m/s
    end
    if numClusters>5
        %figure
        %hold on;
        con_info = zeros(numClusters,2);
        pp.DesiredLinearVelocity=0.3;
        for i = 1:numClusters
            idx = find(labels == i);
            cuboidModel = pcfitcuboid(roiPtCloud, idx);
            points = getCornerPoints(cuboidModel);
            % plot(points(1,1),points(1,2),'r.','MarkerSize',20);
            % plot(points(2,1),points(2,2),'r.','MarkerSize',20);
            % plot(points(3,1),points(3,2),'r.','MarkerSize',20);
            % plot(points(4,1),points(4,2),'r.','MarkerSize',20);
            con_info(i,:) = sum(points(1:4,1:2))/4;
        
        end
        score = 0;
        for i = 1:numClusters
            %plot(con_info(i,1),con_info(i,2),'b.','MarkerSize',20);
            
        end
        % -2 ~ -3 범위의 y 좌표를 가지는 장애물 필터링
        filteredObstacles = con_info(con_info(:,2) >= -3 & con_info(:,2) <= 0, :);
        
        % x 좌표에 따라 정렬
        sortedObstacles = sortrows(filteredObstacles, 1);
        
        % 장애물 간의 거리 계산
        distances = sqrt(diff(sortedObstacles(:,1)).^2 + diff(sortedObstacles(:,2)).^2);
        
        for i = 1 : size(distances,1)
            if distances(i)>4
                parkingSpot = sortedObstacles(i,:) + sortedObstacles(i+1,:);
                parkingSpot = parkingSpot/2;
                parkingSpot(2) = parkingSpot(2)-1.5;
                %plot(parkingSpot(1),parkingSpot(2),'g.','MarkerSize',20);
                detectParkingSpace = false;
                break;
            end
        end    
    end
    [v, w] = pp(vehiclePose);  % Pass the current vehicle pose to the path planner
    [pub, msg] = publish_twist_command(v, w, '/cmd_vel');
    send(pub, msg);
end
[pub, msg] = publish_twist_command(0, 0, '/cmd_vel');
send(pub, msg);

%주차 알고리즘 시작

% figure;
lidarData = receive(lidarSub);
roiPtCloud = preprocess_lidar_data(lidarData, params, roi);

% 이진 맵 생성을 위한 그리드 초기화
xRange = -3:0.1:20;
yRange = -7:0.1:7;
binaryMap = zeros(length(yRange), length(xRange));

% 포인트 클라우드 데이터를 이진 맵으로 변환
for i = 1:size(roiPtCloud.Location, 1)
    x = roiPtCloud.Location(i, 1);
    y = -roiPtCloud.Location(i, 2);
    z = roiPtCloud.Location(i,3);
    
    % 좌표가 그리드 셀에 해당하는지 확인
    xIdx = find(xRange <= x, 1, 'last');
    yIdx = find(yRange <= y, 1, 'last');
    
    % 이진 맵에 포인트 표시
    if ~isempty(xIdx) && ~isempty(yIdx) && z > -0.4
        binaryMap(yIdx, xIdx) = 1;
    end
end


cellSize = 0.1; % 10cm
costmap = vehicleCostmap(binaryMap, 'CellSize', cellSize);

% 차량 치수 설정
vehicleDims = vehicleDimensions(2.4, 2.2, 1); % 길이, 너비, 휠베이스
vehicleDims.FrontOverhang = 0.3; % 앞 오버행
vehicleDims.RearOverhang = 0.5;  % 뒤 오버행

% 충돌 검사기 설정
costmap.CollisionChecker.VehicleDimensions = vehicleDims;

ccConfig = costmap.CollisionChecker;
ccConfig.NumCircles = 2;
costmap.CollisionChecker = ccConfig;


%costmap.CollisionChecker = vehicleCollChecker('InflationRadius', 0.3);
costmap.CollisionChecker.InflationRadius = 0.8;

%currentPose = [0 0 0]; % [x, y, theta]

currentPose = [2 7 0]; %라이다 위치기준에서 후륜축 위치 기준으로
parkMotionPlanner = pathPlannerRRT(costmap, MinIterations=1000);

% Define desired pose for the parking spot, returned by the V2X system.
parkingSpot(1:2) = parkingSpot(1:2)+ currentPose(1:2);
parkPose = [parkingSpot 10];
preParkPose = currentPose;



% Change the connection method to allow for reverse motions.
parkMotionPlanner.ConnectionMethod = "Reeds-Shepp";

parkMotionPlanner.MinTurningRadius   = 5; % meters
parkMotionPlanner.ConnectionDistance = 7; % meters
replan = true;

while replan
    tic
    refPath = plan(parkMotionPlanner, preParkPose, parkPose);
    toc
    % The path corresponding to the parking maneuver is small and requires
    % precise maneuvering. Instead of interpolating only at transition poses,
    % interpolate more finely along the length of the path.
    if refPath.Length == 0
        continue
    end
    numSamples = refPath.Length*2;
    stepSize   = refPath.Length / numSamples;
    lengths    = 0 : stepSize : refPath.Length;
    
    [transitionPoses, directions] = interpolate(refPath, lengths);
    % if (refPath.Length > 0)
    %     k = k;
    % end
    %plotParkingManeuver(costmap, refPath, preParkPose, parkPose)

    % Replan if the path contains more than one direction switching poses
    % or if the path is too long.
    sum(abs(diff(directions(1:end-1))))
    refPath.Length
    replan = sum(abs(diff(directions(1:end-1))))>7 || refPath.Length > 50|| refPath.Length  == 0;
end

%plotParkingManeuver(costmap, refPath, preParkPose, parkPose)

% Smooth the path.
approxSeparation = 0.3; % meters


numSmoothPoses   = round(refPath.Length / approxSeparation);
minSeparation = 0.1;
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses, minSeparation);
% Generate velocity profile.
velMsg = receive(velSub);
currentVel = sqrt(velMsg.Twist.Twist.Linear.X.^2 + velMsg.Twist.Twist.Linear.Y.^2);
refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 0.3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 주차 경로  월드 좌표로 변환

refPoses = refPoses- currentPose ;
%refPoses(1) = refPoses(1)- 0.3;
vehiclePose = getPose(gpsSub, imuSub);
vehiclePose(1) = vehiclePose(1)-0.5   ;
refPoses(:,1:2) = transformWaypointsToOdom(refPoses(:,1:2), vehiclePose);

markerIdPath = 0;


parkPose = parkPose-currentPose;
parkPose(1:2) = transformWaypointsToOdom(parkPose(1:2), vehiclePose);

%%%%%%%%%%%%%%%%% 경로 추종
%sampleTime = 2;
pathAnalyzer = HelperPathAnalyzer(Wheelbase=vehicleDims.Wheelbase);
pathAnalyzer.RefPoses     = refPoses;
pathAnalyzer.Directions   = directions;
pathAnalyzer.VelocityProfile = refVelocities;
pathAnalyzer.Curvatures = curvatures;


figure
hold on
plot(refPoses(:,1),refPoses(:,2));
isGoalReached = false;
v = 0;
currentDir = 1;
while ~isGoalReached
    % Get current driving direction.
    
    tic
    currentPose = getPose(gpsSub, imuSub)';
    currentPose(1) = currentPose(1) - 0.5*cos(currentPose(3));
    currentPose(2) = currentPose(2) - 0.5*sin(currentPose(3));
    theta = currentPose(3);
    currentPose(3)= rad2deg(currentPose(3));
    

    % Find the reference pose on the path and the corresponding velocity.
    
    [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);
    
    plot(refPose(1),refPose(2),'r.','MarkerSize',20);
    plot(currentPose(1),currentPose(2),'b.','MarkerSize',20);

    % If the vehicle changes driving direction, reset vehicle velocity in
    % the simulator and reset longitudinal controller.
    v = refVel;

    if currentDir ~= direction
        %currentVel = 0;
        if direction == -1
            v = -0.1;
        else 
            v = 0.1;
        end

        while currentVel > 0.03 || currentVel <-0.03
            [pub, msg] = publish_twist_command(0, 0, '/cmd_vel');
            send(pub, msg);
            velMsg = receive(velSub);
            currentVel = sqrt(velMsg.Twist.Twist.Linear.X.^2 + velMsg.Twist.Twist.Linear.Y.^2);
        
        end
        while abs(currentVel) +abs( v) <0.14
            [pub, msg] = publish_twist_command(v, v*2, '/cmd_vel');
            send(pub, msg);
            velMsg = receive(velSub);

            currentVel = sqrt(velMsg.Twist.Twist.Linear.X.^2 + velMsg.Twist.Twist.Linear.Y.^2);
            currentVel = currentVel * v * 10;
        end
        currentDir = v * 10;
        %continue 
        currentPose = getPose(gpsSub, imuSub)';
        currentPose(1) = currentPose(1) - 0.5*cos(currentPose(3));
        currentPose(2) = currentPose(2) - 0.5*sin(currentPose(3));
        theta = currentPose(3);
        currentPose(3)= rad2deg(currentPose(3));
        
    end

    % Update driving direction for the simulator. If the vehicle changes
    % driving direction, reset and return the current vehicle velocity as zero.
    %currentVel = updateDrivingDirection(vehicleSim, direction, currentDir);
    % if  currentPose(3) <0
    %      currentPose(3) = 180- currentPose(3);
    % end
    % Compute steering command.
    

    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        Direction=direction, Wheelbase=vehicleDims.Wheelbase);

    % Compute acceleration and deceleration commands.
    %lonController.Direction = direction;
    % [accelCmd, decelCmd] = lonController(refVel, currentVel);
    % v = v* 0.5 + accelCmd + decelCmd;

    w = deg2rad(steeringAngle);
    %w = v * tan(w);
    if w > 0.4
        w = 0.4;
    elseif w < -0.4
        w = -0.4;
    end
    if v <0 && v >-0.15
        v= -0.15;
    end
    [pub, msg] = publish_twist_command(v, w, '/cmd_vel');
    send(pub, msg);
    
    %isGoalReached = sqrt(sum((currentPose(1:2)-parkPose(1:2)).^2)) < 0.5;
    isGoalReached = currentPose(2)<parkPose(2);
    % Check if the vehicle reaches the goal.
    %isGoalReached = sqrt(sum((currentPose(1:2)-parkPose(1:2)).^2)) < 0.3;
    %helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);
   
    
    % Get current pose and velocity of the vehicle.
    %currentPose  = getVehiclePose(vehicleSim);
    
    velMsg = receive(velSub);
    currentVel = sqrt(velMsg.Twist.Twist.Linear.X.^2 + velMsg.Twist.Twist.Linear.Y.^2);
    
    % 차량이 향하는 방향의 단위 벡터
    directionVector = [cos(theta), sin(theta)]; % z 방향은 0으로 가정
    
    % 위치 변화량 (deltaX, deltaY, deltaZ)
    positionChange = [velMsg.Twist.Twist.Linear.X, velMsg.Twist.Twist.Linear.Y]; 
    
    % 전진 또는 후진 결정
    dotProduct = dot(directionVector, positionChange);

    currentVel = currentDir * currentVel;
    toc
end

[pub, msg] = publish_twist_command(0, 0, '/cmd_vel');
send(pub, msg);



%%

function vehiclePose = getPose(gpsSub, imuSub)
    gpsMsg = receive(gpsSub);
    imuMsg = receive(imuSub);
    quat = [imuMsg.Orientation.W, imuMsg.Orientation.X, imuMsg.Orientation.Y, imuMsg.Orientation.Z];
    euler = quat2eul(quat);
    yaw = euler(1);
    vehiclePose = [gpsMsg.Pose.Pose.Position.X;gpsMsg.Pose.Pose.Position.Y;yaw];
end


function [pub, msg] = publish_twist_command(v, w, topicName)
    pub = rospublisher(topicName, 'geometry_msgs/Twist','DataFormat','struct');
    msg = rosmessage(pub);
    msg.Linear.X = v;
    msg.Angular.Z = w;
end

function roiPtCloud = preprocess_lidar_data(lidarData, params, roi)
    xyzData = rosReadXYZ(lidarData);
    ptCloud = pointCloud(xyzData);

    ptCloudOrg = pcorganize(ptCloud, params);

    groundPtsIdx = segmentGroundFromLidarData(ptCloudOrg);
    nonGroundPtCloud = select(ptCloudOrg, ~groundPtsIdx, 'OutputSize', 'full');

    indices = findPointsInROI(nonGroundPtCloud, roi);
    roiPtCloud = select(nonGroundPtCloud, indices);

    roiPtCloud = pcdenoise(roiPtCloud, 'PreserveStructure', true);
end


function odomWaypoints = transformWaypointsToOdom(waypoints, vehiclePoseInOdom)
    % Initialize transformed waypoints
    odomWaypoints = zeros(size(waypoints));

    % Extract the vehicle's yaw angle
    theta = vehiclePoseInOdom(3);

    % Create the 2D rotation matrix
    R = [cos(theta), -sin(theta);
         sin(theta), cos(theta)];

    % Transform each waypoint
    for i = 1:size(waypoints,1)
        % Rotate the waypoint considering the vehicle's yaw
        rotatedPoint = R * waypoints(i,:)';

        % Translate considering the vehicle's position in the odom frame
        odomWaypoints(i,:) = rotatedPoint' + vehiclePoseInOdom(1:2)';
    end
end

일단 전체 코드이고 천천히 설명하겠습니다.

주차 표지판을 보면 실행되는 코드이고, 아직 주차 표지판을 학습하지 못해서 라이다 상 물체가 잡히면 실행되도록 되어 있습니다.

라이다 상 물체가 잡히면 차량은 속도를 확 줄이고, 차량 우측에 콘을 잡아냅니다. 콘이 일정하게 가다가 콘 사이의 거리가 4미터 이상이 되면 그 곳이 주차 지점이라고 판단하고 멈추게 됩니다. 4미터가 떨어진 콘 중간의 중간 좌표에 우측으로 1.5미터 가게 되면 최종 주차 지점이 형성됩니다.

우측에 일렬로 위치한 콘 좌표들

콘들간의 거리

빨간 점이 콘을 인지하고, 박스를 쳐서 4개의 꼭짓점이고, 초록색 점이 제가 주차를 진행할 좌표가 됩니다.

그럼 이제 콘을 인지했으니 주차를 위한 알고리즘을 시작하게 됩니다.

인지된 콘으로 costmap을 만들고, Reeds Shepp을 활용하여 콘에 부딪히지 않게 경로를 형성하게 됩니다.

경로는 차량의 길이, 너비, 최소 회전반경을 기준으로 형성됩니다.

저 직사각형보다 차가 훨씬 작아서 차량이 부딛히지 않는 경로입니다 ㅎㅎ...

그 이후 횡 방향 컨트롤러로 스텐리를 사용하여 경로를 추종합니다.

이건 다른 영상이긴 한데 주차에 성공한 영상입니다.

초반에 오랜시간 matlab에서 계산하는 과정은 컴퓨팅 속도가 차량속도를 따라잡지 못해서 생기는 오류 때문에 회전반경을 크게 해놔서 경로 생성이 오래걸리는 것 때문에 조금 걸리나 지금은 해결해서 3~10초면 생성합니다.