[matlab] rrt구현

% customRRT.m
% 기본 RRT 함수 정의

function [path, tree] = customRRT(map, start, goal, maxIter, stepSize, goalSampleRate)
    % 1) 트리 구조체를 초기화한다. ( 시작 노드만 포함시킴 )
    tree.nodes  = start; % nodes( 1 , : )에 start의 좌표를 저장한다.
    tree.parent = 0; % start노드의 부모 인덱스는 0이다.
    tree.cost   = 0; % 시작점까지의 누적 거리는 0
    % 2) 최대 반복 회수인 maxIter만큼 샘플링을 반복한다.
    for iter = 1:maxIter
        % 2-1) 목표 샘플링을 할지 안할지 결정한다.
        if rand < goalSampleRate
            sample = goal; % 만약 rand가 goalSampleRate보다 작다면, sample에 goal할당
        else
            xlim = map.XLocalLimits; % 맵에서 [xmin , xmax]을 가져온다.
            ylim = map.YLocalLimits; % 맵에서 [ymin , ymax]를 가져온다.
            % rand * ( max-min ) + min형태로 좌표를 생성한다.
            % matlab에서 rand함수는 0과 1사이의 균등 분포 난수를 생성한다.
            sample = [ rand*(xlim(2)-xlim(1))+xlim(1), ...
                       rand*(ylim(2)-ylim(1))+ylim(1) ];
        end
        % 2-2) 트리에서 sample에서 가장 가까운 노드를 찾는다.
        diffs = tree.nodes - sample; % 현재 트리의 모든 노드를과 샘플링 된 점간의 좌표 차이를 계산하는 부분.
        dists = hypot(diffs(:,1), diffs(:,2)); % 차이 벡터 길이( 거리 )를 계산한다.
        % min은 ( 값 , 인덱스 )을 리턴한다.여기서 값은 필요 없으니 ~로 받는다. []라고 써있어서 배열로 받는게
        % 아니라, 각각의 변수를 의미하는것이다.
        [~, idxNearest] = min(dists); % 최소 거리의 인덱스를 추출한다. 
        nearest = tree.nodes(idxNearest, :); % tree의 nodes에서 idxNearest에 해당되는 인덱스의 노드에서 모든열을 가져온다. :를 사용하여 표현한다.
        
        % 2-3) steer(extend) : nearest에서 sample방향으로 최대 stepSize만큼 이동한다.
        % 단위 방향 벡터를 계산한다. (sample-nearest)는 두 점간의 벡터를 의미한다.
        % norm은 벡터의 길이를 계산하는 것이다.
        direction = (sample - nearest) / norm(sample - nearest);
        newNode = nearest + stepSize * direction;

        % 충돌 검사
        if ~checkCollision(map, nearest, newNode)
            % 노드 추가
            tree.nodes(end+1, :)  = newNode;
            tree.parent(end+1, 1) = idxNearest;
            tree.cost(end+1, 1)   = tree.cost(idxNearest) + norm(newNode - nearest);
            % 목표 도달 확인
            if norm(newNode - goal) < stepSize
                path = tracePath(tree, size(tree.nodes,1));
                disp('goal')
                return;
            end
        end
    end
    path = [];  % 실패 시 빈 반환
end

function coll = checkCollision(map, p1, p2)
    % map.Resolution 기준으로 p1 -> p2 직선 구간을 샘플링해서 occupied검사를 한다
    % map.Resolution은 격자 해상도를 나타내는 속성이다. 구체적으로는 1m당 몇 개의 셀(cell)이 있는지를 숫자로
    % 저장한다.
    % 셀 하나의 실제 크기는 cellSize = 1/map.Resolution이다.
    % 만약에 map.Resolution이 10이라면 cellSize = 1/10 = 0.1m이다.
    dist = norm(p2 - p1); % 두 점사이의 거리를 구한다.
    % 샘플링할 포인트 개수인 n을 결정
    % dist/cellSize는 두 점 사이를 지나는 총 셀의 개수이다.
    % *2를 해서 각 셀의 경계마다 두번 샘플링하여 더 자세히 검사
    cellSize = 1/map.Resolution
    n = ceil(dist / cellSize * 2); 
    % p1과 p2를 잇는 직선을 n개의 지점으로 균등 분할하여 좌표 샘플을 생성한다.
    % linspace( a , b , c )는 a부터 b까지를 포함해서, 균등한 간격으로 n개의 숫자를 생성하는 함수
    xs = linspace(p1(1), p2(1), n);
    ys = linspace(p1(2), p2(2), n);
    coll = false;
    for i = 1:n
        if getOccupancy(map, [xs(i), ys(i)]) > 0.5 
            coll = true;
            disp('crush')
            return;
        end
    end
end

function path = tracePath(tree, idx)
    % goal 노드에서부터 부모 인덱스를 따라 start까지 경로 역추적
    path = tree.nodes(idx, :);
    while tree.parent(idx) ~= 0
        idx  = tree.parent(idx);
        path = [tree.nodes(idx, :); path]; %#ok<AGROW>
    end
end

% runRRT.m
load exampleMaps.mat                   % simpleMap 로드
map = occupancyMap(simpleMap, 10);    % 해상도 10 → 셀 크기 0.1m

start = [0.5, 0.5];  % 시작 위치 (x, y)
goal  = [2.5, 0.2];  % 목표 위치 (x, y)

% 재현 가능한 결과 위해 시드 고정
rng(100, 'twister');

% customRRT 호출: 5000회 반복, 한 분기당 0.3m, 10% 확률로 목표 샘플링
[path, tree] = customRRT(map, start, goal, 5000, 0.3, 0.1);

% 결과 시각화
show(map); hold on;
plot(tree.nodes(:,1), tree.nodes(:,2), '.-');              % 트리 확장
plot(path(:,1), path(:,2), 'r-', 'LineWidth', 2);          % 찾은 경로