#! /bin/bash

source devel/setup.bash

case $1 in
    "t2" )
        wait-for-it localhost:11311 -- echo "ready"
        roslaunch rois_gazebo spawn_human.launch
        ;;
    "t3" )
        wait-for-it localhost:11311 -- echo "ready"
        roscd route_guidance_ros/scripts/
        while [ -z `rostopic list|grep /human1` ]
        do 
            echo "waiting human1"
            sleep 1
        done 
        python3 -c "import setup_comp_servers;setup_comp_servers.setup_single_robot();input('hit Enter key')"
        ;;
    "t3m" )
        wait-for-it localhost:11311 -- echo "ready"
        roscd route_guidance_ros/scripts/
        while [ -z `rostopic list|grep /human1` ]
        do 
            echo "waiting human1"
            sleep 1
        done
        python3 -c "import setup_comp_servers;setup_comp_servers.setup_multi_robot();input('hit Enter key')"
        ;;
    "t4" )
        roscd route_guidance_ros/scripts/
        python3 setup_engines.py
        ;;
    "t4m" )
        wait-for-it localhost:11311 -- echo "ready"
        roscd route_guidance_ros/scripts/
        python3 wait_server.py client_info_2.json comp
        python3 setup_engines_multi.py
        ;;
    "t5" )
        wait-for-it localhost:11311 -- echo "ready"
        roscd route_guidance_ros/scripts/
        python3 wait_server.py client_info_2.json comp
        python3 wait_server.py client_info_2.json eng
        wait-for-it localhost:8000 -- echo "ready main HRI Engine"
        python3 sa_multi.py
        ;;
    "t5a" )
        wait-for-it localhost:8000 -- echo "ready"
        sleep 5
        rostopic pub -1 human1/diff_drive_controller/cmd_vel geometry_msgs/Twist -- "[0,0,0]" "[0,0,-1]"
        rostopic pub -1 human1/diff_drive_controller/cmd_vel geometry_msgs/Twist -- "[1,0,0]" "[0,0,0]"
        rostopic pub -1 human1/diff_drive_controller/cmd_vel geometry_msgs/Twist -- "[1,0,0]" "[0,0,0]"
        ;;
    "t5o" )
        rostopic pub -1 robot1/detected_text std_msgs/String "data: 'point_a'"
        ;;
esac