# Nav2 Collision Monitor: 마지막 안전 게이트.
#
# terrain_speed_node가 낸 /cmd_vel_terrain(Nav2 순찰/자율주행 + parking_controller의
# 도킹/순찰 명령 모두 여기로 합쳐짐)을 받아서, /scan 라이다 데이터를 기준으로 갑자기
# 튀어나온 사람/장애물이 있으면 감속하거나(SlowZone) 완전 정지시킨(StopZone) 뒤 최종
# /cmd_vel로 내보낸다. FootprintApproach는 현재 속도 명령대로 주행했을 때
# time_before_collision초 안에 로봇 발자국(costmap footprint)이 장애물과 닿는지
# 시뮬레이션해서 미리 감속하는 예측형 정지 로직이다.
#
# 로봇 사양(fori_nav2_params.yaml의 robot_radius: 0.38m, 순항속도 0.3m/s)에 맞춰
# 반경/시간을 잡았다. 실제 부지에서 저속 시운전하며 StopZone/SlowZone 반경과
# slowdown_ratio를 조정할 것을 권장.
collision_monitor:
  ros__parameters:
    use_sim_time: False
    base_frame_id: "base_link"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_terrain"
    cmd_vel_out_topic: "cmd_vel"
    transform_tolerance: 0.5
    source_timeout: 1.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    polygons: ["StopZone", "SlowZone", "FootprintApproach"]
    # 로봇 중심에서 0.45m(로봇 반경 0.38m + 여유 0.07m) 이내에 장애물이 잡히면 완전 정지.
    StopZone:
      type: "circle"
      radius: 0.45
      action_type: "stop"
      max_points: 3
      visualize: True
      polygon_pub_topic: "collision_monitor/stop_zone"
      enabled: True
    # 0.90m 이내로 접근하면 지령 속도를 40%로 줄여 미리 서행.
    SlowZone:
      type: "circle"
      radius: 0.90
      action_type: "slowdown"
      slowdown_ratio: 0.4
      max_points: 3
      visualize: True
      polygon_pub_topic: "collision_monitor/slow_zone"
      enabled: True
    # 현재 속도 명령으로 1.5초 앞을 시뮬레이션해서 로봇 발자국이 장애물과 겹치면
    # 미리 감속 (local_costmap이 robot_radius 기준으로 자동 발행하는 발자국 사용).
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 1.5
      simulation_time_step: 0.1
      max_points: 3
      visualize: False
      enabled: True
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "/scan"
      enabled: True
