teb 코드와 함께 보고서 작성 준비하기

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



% Parameter//=============================================================
roi = [0, 7, -7, 7, -2, 4];
NumClusterPoints = 200;


pp=controllerPurePursuit;
pp.LookaheadDistance =4; % m
pp.DesiredLinearVelocity = 0.2; % m/s
pp.MaxAngularVelocity = 0.4; % rad/s


worldFrame = 'erp42_base';

Lidar = true;
GpsImu = true;

% init//==================================================================
% load('groundWaypoint.mat');
% waypoints = zeros(size(groundWaypoint)); % longitude latitude
% load('waypoints.mat');

waypoints = [0:0.5:40; zeros(1, length(0:0.5:40))-0.5]';

if Lidar
    params = lidarParameters('OS1Gen1-64',512);
    sub.Lidar = rossubscriber('/lidar/points',"DataFormat","struct");
end

if GpsImu
    sub.Gps = rossubscriber("/gps/fix","sensor_msgs/NavSatFix","DataFormat","struct");
    sub.Imu = rossubscriber("/imu/data","sensor_msgs/Imu","DataFormat","struct");
    vehiclePose_origin = getVehiclePose_gps(sub); % [x_utm, y_utm, euler(1)]
else
    sub.Odom = rossubscriber("/odom_skid","nav_msgs/Odometry","DataFormat","struct");
    vehiclePose_origin = getVehiclePose_odom(sub);
end


% Publish Command to Hunter
pub.Cmd = rospublisher('/cmd_vel', 'geometry_msgs/Twist','DataFormat','struct');

% '/test_optim_node/local_plan' 토픽 구독
pathSub = rossubscriber('/test_optim_node/local_plan', 'nav_msgs/Path');

% Obstacle Publsih to msg_convert
pub.Obs_convert = rospublisher('/matlab/obstacle', 'sensor_msgs/Imu');

% Obstacle Visualize Publisher
pub.Obs_visual = rospublisher('/matlab/obstacl_visual', 'visualization_msgs/MarkerArray');

markerArrayMsg = rosmessage('visualization_msgs/MarkerArray');
markerMsg = rosmessage('visualization_msgs/Marker');
Obs_visualID = 0;

% % ground waypoint utm -> erp-42 coordinate
% for i = 1:size(groundWaypoint,1)
%     [a,b]= projfwd(projcrs(32652), groundWaypoint(i,2), groundWaypoint(i,1));
%     waypoints(i,1) = a;
%     waypoints(i,2) = b;
% end
% waypoints = transformWaypointsToWorld(waypoints, vehiclePose_origin);

% Path Visualize Publisher
pub.Path_visual = rospublisher('/matlab/path_visual', 'geometry_msgs/PoseArray');
poseHistoryMsg = rosmessage('geometry_msgs/PoseArray');

% Global path Publish to TEB Planner
pathPub = rospublisher('/matlab/globalPath', 'nav_msgs/Path');
pathMsg = rosmessage(pathPub);
currentTime = rostime('now');
for i = 1:size(waypoints, 1)
    pose = rosmessage('geometry_msgs/PoseStamped');
    pose.Header.FrameId = worldFrame; 
    pose.Header.Stamp = currentTime;
    pose.Pose.Position.X = waypoints(i, 1);
    pose.Pose.Position.Y = waypoints(i, 2);
    pose.Pose.Orientation.W = 1; % Quaternion (0,0,0,1)은 회전 없음을 의미
    pathMsg.Poses(i) = pose;
end
pathMsg.Header.FrameId = worldFrame;
pathMsg.Header.Stamp = currentTime;

send(pathPub, pathMsg);


while true
    tic;
    if GpsImu
        vehiclePose = getVehiclePose_gps(sub); % get pose data from gps, imu
        vehiclePose_abs = vehiclePose;

        % 초기 위치에서 현재 위치까지의 변위 계산
        dx = vehiclePose(1) - vehiclePose_origin(1);
        dy = vehiclePose(2) - vehiclePose_origin(2);

        % 초기 회전(yaw)을 기준으로 회전 변환 적용
        cos_yaw = cos(-vehiclePose_origin(3));
        sin_yaw = sin(-vehiclePose_origin(3));
        relativeX = cos_yaw * dx - sin_yaw * dy;
        relativeY = sin_yaw * dx + cos_yaw * dy;

        % 상대적 yaw 각도 계산
        relativeYaw = vehiclePose(3) - vehiclePose_origin(3);

        % 결과 벡터 반환
        vehiclePose = [relativeX, relativeY, relativeYaw];
    else
        vehiclePose = getVehiclePose_odom(sub); % get pose from odom
    end

    broadcastTftree(vehiclePose,tftree,worldFrame); % broadcasting TF tree


    if Lidar
        lidarData = receive(sub.Lidar);
        
        % LIDAR 데이터를 PointCloud로 변환
        xyzData = rosReadXYZ(lidarData);
        ptCloud = pointCloud(xyzData);

        % OUSTER 초기 파라미터 탐지범위의 포인트 클라우드 설정
        ptCloudOrg = pcorganize(ptCloud,params); 
        
        % % 지면을 라이다로 탐지하지 않기 위해 지면 분할 수행
        groundPtsIdx = segmentGroundFromLidarData(ptCloudOrg);
        groundPtCloud = select(ptCloudOrg, groundPtsIdx);
        nonGroundPtCloud = select(ptCloudOrg, ~groundPtsIdx, 'OutputSize', 'full');
        
        % % 탐지범위내에서 지면이 아닌 곳에서 물체탐지위한 포인트클라우드 설정 및 노이즈 제거
        indices = findPointsInROI(nonGroundPtCloud,roi);
        roiPtCloud = select(nonGroundPtCloud,indices);
        roiPtCloud = pcdenoise(roiPtCloud,PreserveStructure=true,NumNeighbors=30);
           
        % 포인트클라우드 N 이상일때만 물체로 판단
        [labels, numClusters] = pcsegdist(roiPtCloud,0.3,"NumClusterPoints",NumClusterPoints);
        % 초기화: 물체 정보를 저장할 구조체 배열 생성
        obstacle_Info = struct('Center',struct('x', [], 'y',[], 'z' ,[] ),'Dimensions', struct('l', [], 'w',[], 'h' ,[] ), 'Orientation', []);


        % 탐지된 물체 순회
        for i = 1:numClusters
            idx = find(labels == i);
            cuboidModel = pcfitcuboid(roiPtCloud, idx);
            points = getCornerPoints(cuboidModel);          


            % 고유한 ID를 생성
            obstacleID = Obs_visualID;
            Obs_visualID = Obs_visualID + 1;

            % 차량의 회전 행렬 생성
            R = [cos(vehiclePose(3)), -sin(vehiclePose(3));
                 sin(vehiclePose(3)), cos(vehiclePose(3))];
        

            % 각 꼭짓점 변환
            v1_world = R * [points(1,1); points(1,2)] + [vehiclePose(1); vehiclePose(2)];
            v2_world = R * [points(2,1); points(2,2)] + [vehiclePose(1); vehiclePose(2)];
            v3_world = R * [points(3,1); points(3,2)] + [vehiclePose(1); vehiclePose(2)];
            v4_world = R * [points(4,1); points(4,2)] + [vehiclePose(1); vehiclePose(2)];


            % Publis obstacle to msg_convert
            obstaclePose = rosmessage('sensor_msgs/Imu');
            obstaclePose.Orientation.X = v1_world(1);  obstaclePose.Orientation.Y = v1_world(2);
            obstaclePose.Orientation.Z = v2_world(1);  obstaclePose.Orientation.W = v2_world(2);
            obstaclePose.AngularVelocity.X = v3_world(1); obstaclePose.AngularVelocity.Y = v3_world(2);
            obstaclePose.AngularVelocity.Z = v4_world(1); obstaclePose.LinearAcceleration.X = v4_world(2);

            send(pub.Obs_convert, obstaclePose);  
            
            % Visualization
            % 고유한 ID 할당
            markerMsg = rosmessage('visualization_msgs/Marker');
            markerMsg.Id = Obs_visualID;
            Obs_visualID = Obs_visualID+1;

            % Marker 설정
            markerMsg.Header.FrameId = worldFrame;
            markerMsg.Type = 1;
            markerMsg.Scale.X = cuboidModel.Dimensions(1);
            markerMsg.Scale.Y = cuboidModel.Dimensions(2);
            markerMsg.Scale.Z = cuboidModel.Dimensions(3);

            % 위치 설정
            markerMsg.Pose.Position.X = (v1_world(1) + v2_world(1) + v3_world(1) + v4_world(1)) / 4;
            markerMsg.Pose.Position.Y = (v1_world(2) + v2_world(2) + v3_world(2) + v4_world(2)) / 4;
            markerMsg.Pose.Position.Z = 0;

            % 색상 설정
            markerMsg.Color.R = 0; % 빨간색 (0)
            markerMsg.Color.G = 0; % 녹색 (0)
            markerMsg.Color.B = 0.7; % 파란색 (1)
            markerMsg.Color.A = 1; % 알파 값 (투명도) 설정

            % ROS 메시지에 쿼터니언 대입
            markerMsg.Pose.Orientation.Z = sin((deg2rad(cuboidModel.Orientation(3)) + vehiclePose(3)) / 2);
            markerMsg.Pose.Orientation.W = cos((deg2rad(cuboidModel.Orientation(3)) + vehiclePose(3)) / 2);

            % MarkerArray에 Marker 추가
            markerArrayMsg.Markers(end+1) = markerMsg;

        end
        % 토픽 발행
        send(pub.Obs_visual, markerArrayMsg);
    
    toc;
    % 메시지 수신
    pathMsg = receive(pathSub);
    
    % 웨이포인트 배열 초기화
    waypoints = zeros(length(pathMsg.Poses), 2);
    
    % 각 포즈에서 웨이포인트 추출
    for i = 1:length(pathMsg.Poses)
        waypoints(i, 1) = pathMsg.Poses(i).Pose.Position.X;
        waypoints(i, 2) = pathMsg.Poses(i).Pose.Position.Y;
    end
    pp.Waypoints = waypoints;

    % 주행 경로 시각화
    poseArrayMsg = rosmessage('geometry_msgs/PoseArray');
    poseArrayMsg.Header.FrameId = worldFrame;
    pose = rosmessage('geometry_msgs/Pose');
    pose.Position.X = vehiclePose(1);
    pose.Position.Y = vehiclePose(2);
    pose.Orientation.Z = sin(vehiclePose(3)/2);
    pose.Orientation.W = cos(vehiclePose(3)/2);
    % 이전 포즈 저장
    poseHistoryMsg.Poses(end+1) = pose;
    % PoseArray 메시지에 이전의 모든 포즈 추가
    poseArrayMsg.Poses = poseHistoryMsg.Poses;
    send(pub.Path_visual, poseArrayMsg);
    
    vehiclePose(1) = vehiclePose(1)+ 0.5* cos(vehiclePose(3));
    vehiclePose(2) = vehiclePose(2)+ 0.5* sin(vehiclePose(3));
    [v, w] = pp(vehiclePose);  % Pass the current vehicle pose to the path planner
    cmdMsg = publish_twist_command(v, w, pub.Cmd);
    send(pub.Cmd, cmdMsg);
    end
end


%%

function odomWaypoints = transformWaypointsToWorld(waypoints, vehiclePose)
    % Initialize transformed waypoints
    odomWaypoints = zeros(size(waypoints));

    % Extract the vehicle's yaw angle
    theta = vehiclePose(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' + vehiclePose(1:2);
    end
end


function vehiclePose = getVehiclePose_odom(sub)
    msg = receive(sub.Odom);

    % 위치 정보 추출
    x = msg.Pose.Pose.Position.X;
    y = msg.Pose.Pose.Position.Y;

    % 방향(오리엔테이션) 정보 추출 및 Yaw 각도 계산
    quat = [msg.Pose.Pose.Orientation.W, msg.Pose.Pose.Orientation.X, ...
            msg.Pose.Pose.Orientation.Y, msg.Pose.Pose.Orientation.Z];
    eul = quat2eul(quat);
    yaw = eul(1); 
    vehiclePose = [x,y,yaw];
end


function vehiclePose = getVehiclePose_gps(sub)
    gpsMsg = receive(sub.Gps);
    imuMsg = receive(sub.Imu);
    quat = [imuMsg.Orientation.W,imuMsg.Orientation.X,imuMsg.Orientation.Y,imuMsg.Orientation.Z];
    euler = quat2eul(quat);
    % UTM 좌표계로 변환 (Korea: 32652)
    [x_utm, y_utm] = projfwd(projcrs(32652), gpsMsg.Latitude, gpsMsg.Longitude);

    vehiclePose = [x_utm, y_utm, euler(1)];
end


function broadcastTftree(vehiclePose,tftree,worldFrame)
    % TF 메시지 생성 및 설정
    tfStampedMsg = rosmessage('geometry_msgs/TransformStamped');
    tfStampedMsg.ChildFrameId = 'base_link';
    tfStampedMsg.Header.FrameId = worldFrame;
    tfStampedMsg.Header.Stamp = rostime('now');
    tfStampedMsg.Transform.Translation.X = vehiclePose(1);
    tfStampedMsg.Transform.Translation.Y = vehiclePose(2);
    tfStampedMsg.Transform.Rotation.Z = sin(vehiclePose(3)/2);
    tfStampedMsg.Transform.Rotation.W = cos(vehiclePose(3)/2);
    
    sendTransform(tftree, tfStampedMsg);
end


function cmdMsg = publish_twist_command(v, w, pub)
    cmdMsg = rosmessage(pub);
    cmdMsg.Linear.X = v;
    cmdMsg.Angular.Z = w;
end

여기서 matlab == teb네요

밥먹고 작성하겠습니다..