"""
dynamic_obstacles.launch.py — Dynamic obstacle tracking + Nav2 costmap layer.

Starts:
  dynamic_obs_tracker  — LIDAR motion detection + Kalman tracking @10 Hz
  dynamic_obs_costmap  — Predicted-trajectory → PointCloud2 for Nav2

Launch args:
  max_tracks         int    '20'
  assoc_dist_m       float  '1.5'
  horizon_s          float  '2.5'
  inflation_radius_m float  '0.35'

Verify:
  ros2 topic hz /saltybot/moving_objects        # ~10 Hz
  ros2 topic echo /saltybot/moving_objects      # TrackedObject list
  ros2 topic hz /saltybot/dynamic_obs_cloud     # ~10 Hz (when objects present)
  rviz2 → add MarkerArray → /saltybot/moving_objects_viz

Nav2 costmap integration:
  In your costmap_params.yaml ObstacleLayer observation_sources, add:
    dynamic_obs:
      topic: /saltybot/dynamic_obs_cloud
      data_type: PointCloud2
      marking: true
      clearing: false
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    args = [
        DeclareLaunchArgument('max_tracks',         default_value='20'),
        DeclareLaunchArgument('assoc_dist_m',        default_value='1.5'),
        DeclareLaunchArgument('horizon_s',           default_value='2.5'),
        DeclareLaunchArgument('inflation_radius_m',  default_value='0.35'),
        DeclareLaunchArgument('min_speed_mps',       default_value='0.05'),
    ]

    tracker = Node(
        package='saltybot_dynamic_obstacles',
        executable='dynamic_obs_tracker',
        name='dynamic_obs_tracker',
        output='screen',
        parameters=[{
            'max_tracks':       LaunchConfiguration('max_tracks'),
            'assoc_dist_m':     LaunchConfiguration('assoc_dist_m'),
            'horizon_s':        LaunchConfiguration('horizon_s'),
            'min_speed_mps':    LaunchConfiguration('min_speed_mps'),
            'prediction_hz':    10.0,
            'confirm_frames':   3,
            'max_missed_frames': 6,
            'pred_step_s':      0.5,
            'odom_frame':       'odom',
            'max_range_m':      8.0,
        }],
    )

    costmap = Node(
        package='saltybot_dynamic_obstacles',
        executable='dynamic_obs_costmap',
        name='dynamic_obs_costmap',
        output='screen',
        parameters=[{
            'inflation_radius_m': LaunchConfiguration('inflation_radius_m'),
            'ring_points':        8,
            'clear_on_empty':     True,
        }],
    )

    return LaunchDescription(args + [tracker, costmap])
