#!/bin/bash
# FORI AGV 안전 시작 스크립트
# 이전 프로세스 잔류 문제를 자동으로 처리합니다

echo "=========================================="
echo "     FORI AGV - 안전 시작 스크립트"
echo "=========================================="

# 1. 이전 실행 잔류 프로세스 모두 종료
echo "[1/3] 이전 프로세스 정리 중..."
pkill -9 -f "serial_bridge_node" 2>/dev/null
pkill -9 -f "parking_controller_node" 2>/dev/null
pkill -9 -f "ui_server_node" 2>/dev/null
pkill -9 -f "rosbridge_websocket" 2>/dev/null
pkill -9 -f "web_video_server" 2>/dev/null
pkill -9 -f "aruco_detector_node.py" 2>/dev/null
pkill -9 -f "fori_full.launch" 2>/dev/null
pkill -9 -f "fori_nav2.launch" 2>/dev/null
fuser -k -9 8080/tcp 9090/tcp 8085/tcp 2>/dev/null
sleep 2

echo "[2/3] ROS2 환경 로드..."
source /opt/ros/humble/setup.bash
source /home/yoo/fori_ws/fori_ws/install/setup.bash

# Hikrobot 카메라 SDK(/opt/MVS/lib/64)가 LD_LIBRARY_PATH 맨 앞에 있어서 시스템
# libusb보다 먼저 잡히는데, 그 안의 libusb가 구버전이라 PCL(fast_lio, pcd_gridmap_converter
# 등)이 필요로 하는 libusb_set_option 심볼이 없어 "symbol lookup error"로 죽는다.
# 시스템 라이브러리 경로를 앞에 둬서 심볼 충돌만 피하고, MVS 경로는 카메라 전용 라이브러리
# 탐색을 위해 그대로 뒤에 남겨둔다.
export LD_LIBRARY_PATH="/usr/lib/x86_64-linux-gnu:/lib/x86_64-linux-gnu:${LD_LIBRARY_PATH}"

echo "[3/3] FORI AGV 시작!"
echo "  ✅ 웹 UI: http://localhost:8080"
echo "  ✅ 외부(모바일): http://$(hostname -I | awk '{print $1}'):8080"
echo "=========================================="
ros2 launch fori_serial_bridge fori_full.launch.py