#!/usr/bin/env bash
source /opt/ros/humble/setup.bash
source /workspace/install/setup.bash

echo "[restart] Killing old processes..."
pkill -f "explore_lite/lib/explore_lite" 2>/dev/null || true
pkill -f "nav2_smoother/smoother_server" 2>/dev/null || true
pkill -f "nav2_behaviors/behavior_server" 2>/dev/null || true
pkill -f "nav2_waypoint_follower/waypoint_follower" 2>/dev/null || true
pkill -f "nav2_velocity_smoother/velocity_smoother" 2>/dev/null || true
pkill -f "nav2_controller/controller_server" 2>/dev/null || true
pkill -f "nav2_planner/planner_server" 2>/dev/null || true
pkill -f "nav2_bt_navigator/bt_navigator" 2>/dev/null || true
pkill -f "nav2_lifecycle_manager/lifecycle_manager" 2>/dev/null || true
sleep 2

echo "[restart] Starting Nav2 with fast params..."
nohup ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true params_file:=/workspace/nav2_params_fast.yaml > /tmp/nav2.log 2>&1 &

echo "[restart] Starting explore_lite with fast params..."
nohup ros2 launch explore_lite explore.launch.py use_sim_time:=true > /tmp/explore.log 2>&1 &

echo "[restart] Done. Logs: /tmp/nav2.log /tmp/explore.log"
echo "[restart] Verify in 5s with: ros2 node list | grep -E \"explore|nav2\""