#!/usr/bin/env bash
set -e

WORKSPACE_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
source "${WORKSPACE_DIR}/devel/setup.bash"

USB_CAMERA_INDEX="${CAIM_USB_CAMERA_INDEX:-0}"
USB_CAMERA_NAME="${CAIM_USB_CAMERA_NAME:-}"
USB_CAMERA_TOPIC="${CAIM_USB_CAMERA_TOPIC:-/usb_camera/image_raw}"

# 一只 USB 摄像头只打开一次，下视圆环和 A/B 识别共享该图像话题。
rosrun caim_core usb_camera_node.py \
  _device_name:="${USB_CAMERA_NAME}" \
  _device_index:="${USB_CAMERA_INDEX}" \
  _image_topic:="${USB_CAMERA_TOPIC}" &

# OctoMap 使用 FAST-LIO 的机体系点云和 camera_init -> body TF。
rosrun octomap_server octomap_server_node \
  _frame_id:=camera_init \
  _resolution:=0.05 \
  _sensor_model/max_range:=8.0 \
  _pointcloud_min_z:=-0.20 \
  _pointcloud_max_z:=3.00 \
  cloud_in:=/cloud_registered_body &

roslaunch caim_core box_perception.launch &
roslaunch caim_core downward_visual.launch \
  start_usb_camera:=false \
  image_topic:="${USB_CAMERA_TOPIC}" &
roslaunch caim_core routes.launch show_rviz:=true &
roslaunch caim_core position_node.launch &
roslaunch caim_core target_finding.launch \
  start_usb_camera:=false \
  camera_topic:="${USB_CAMERA_TOPIC}" &
echo "OctoMap、感知、避障、主状态机启动完成"
trap 'kill $(jobs -p) 2>/dev/null' EXIT
wait