marble

#!/bin/bash

# Setup variables to quickly switch between various modes
# Whether to use Ignition or Gazebo
use_ign=true
# Use the simulated comms if true, virtual if false
simcomms=true

enable_ground=true
enable_air=false
enable_voxblox=false
# Both false = No beacons; beacons1 = B01 and B02; beacons2 = B03 - B08
enable_beacons1=false
enable_beacons2=false

if [ "$1" == "start" ]; then
  echo "Starting MARBLE services..."

  # Disable the ground vehicle and turn on air mode
  if [ "$2" == "air_only" ]; then
    enable_ground=false
  fi

  # Default to using the cave environment
  if [ "$2" == "" ]; then
    world="cave"
  elif [ "$2" == "air" ] || [ "$2" == "air_only" ]; then
    enable_air=true
    if [ "$3" == "" ]; then
      world="cave"
    elif [ "$3" == "voxblox" ]; then
      enable_voxblox=true
      if [ "$4" == "" ]; then
        world="cave"
      else
        world=$4
      fi
    else
      world=$3
    fi
  else
    world=$2
  fi


  # Select the world file to run
  if $use_ign; then
    if [ "$world" == "cave" ]; then
      scenario="simple_cave_01"
      ignlaunch="cave_circuit.ign"
    elif [ "$world" == "urban" ]; then
      scenario="urban_circuit_practice_01"
      ignlaunch="urban_circuit.ign"
    elif [ "$world" == "tunnel" ]; then
      scenario="tunnel_circuit_practice_01"
      ignlaunch="tunnel_circuit_practice.ign"
    fi

    # Overwrite the world name if it was passed by command line
    scenario_check=$3
    if [ "$2" == "air" ] || [ "$2" == "air_only" ]; then
      scenario_check=$4
    fi
    if [ "$scenario_check" != "" ]; then
      scenario=$scenario_check
    fi

    ground=""
    if $enable_ground; then
      ground="robotName2:=X1 robotConfig2:=marble_husky_lite robotName3:=X2 robotConfig3:=marble_husky_lite"
    fi
    air=""
    if $enable_air; then
      air="robotName4:=A01 robotConfig4:=marble_qav500_lite"
      # air="robotName4:=A01 robotConfig4:=marble_qav500_sensor_config_1"
    fi

    beacons=""
    if $enable_beacons1; then
      beacons="robotName11:=B01 robotConfig11:=marble_beacon robotName12:=B02 robotConfig12:=marble_beacon"
    fi
    if $enable_beacons2; then
      beacons="robotName11:=B01 robotConfig11:=marble_beacon robotName12:=B02 robotConfig12:=marble_beacon robotName13:=B03 robotConfig13:=marble_beacon robotName14:=B04 robotConfig14:=marble_beacon robotName15:=B05 robotConfig15:=marble_beacon robotName16:=B06 robotConfig16:=marble_beacon robotName17:=B07 robotConfig17:=marble_beacon robotName18:=B08 robotConfig18:=marble_beacon"
    fi
    # Launch Ignition
    # ign launch $ignlaunch worldName:=$scenario robotName1:=X1 robotConfig1:=marble_husky_sensor_config_1 robotName2:=X2 robotConfig2:=marble_husky_sensor_config_1 robotName3:=Base robotConfig3:=X2_SENSOR_CONFIG_1 enableGroundTruth:=1 localModel:=true &
    ign launch -v 3 $ignlaunch worldName:=$scenario robotName1:=Base robotConfig1:=TEAMBASE $ground $air $beacons enableGroundTruth:=1 localModel:=true &
    # ign launch $ignlaunch worldName:=$scenario robotName1:=X1 robotConfig1:=X1_SENSOR_CONFIG_4 robotName2:=X2 robotConfig2:=X1_SENSOR_CONFIG_4 robotName3:=Base robotConfig3:=X2_SENSOR_CONFIG_1  enableGroundTruth:=1 $beacons localModel:=true &
    # ign launch $ignlaunch worldName:=$scenario robotName1:=X1 robotConfig1:=marble_husky_lite robotName2:=X2 robotConfig2:=SSCI_X4_SENSOR_CONFIG_1 robotName3:=Base robotConfig3:=X2_SENSOR_CONFIG_1 enableGroundTruth:=1 localModel:=true &
    sleep 10
  else
    export X1_SENSOR_CONFIG_4=1

    if [ "$3" == "" ]; then
      if [ "$world" == "cave" ]; then
        scenario="tunnel_practice_2"
      elif [ "$world" == "urban" ]; then
        scenario="urban_underground"
      elif [ "$world" == "tunnel" ]; then
        scenario="tunnel_practice_2"
      fi
    else
      scenario=$3
    fi
  fi

  # Run our launch file
  roslaunch ~/marble/scripts/subt.launch use_ign:=$use_ign simcomms:=$simcomms scenario:=$scenario voxblox:=$enable_voxblox ground:=$enable_ground air:=$enable_air beacons1:=$enable_beacons1 beacons2:=$enable_beacons2 &
  sleep 10

  # Run the python scripts
  # TF Transforms
  if ! $use_ign; then
    echo "Starting World to Bot TF..."
    python2 ~/marble/scripts/world_to_bot_tf.py &
  fi
  # Odometry correction
  if $enable_ground; then
    python2 ~/marble/scripts/subt_true_odom.py X1 &
    python2 ~/marble/scripts/subt_true_odom.py X2 &
  fi

  if $enable_air; then
    python2 ~/marble/scripts/subt_true_odom.py A01 &
  fi

  # Start comms
  ground_comm=""
  if $enable_ground; then
    ground_comm="X1 X2"
  fi

  air_comm=""
  if $enable_air; then
    air_comm="A01"
  fi

  if $enable_beacons2; then
    beacon_comm="B01 B02 B03 B04 B05 B06 B07 B08"
  elif $enable_beacons1; then
    beacon_comm="B01 B02"
  else
    beacon_comm=""
  fi

  if $simcomms; then
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_handler.py Base $beacon_comm $ground_comm $air_comm &
  fi

  sleep 5

  # Turn on headlights.  See if we can move to launch file
  # ~/marble/scripts/marble lights

  # Start the comms checker
  if $simcomms; then
    ~/marble/scripts/marble commscheck $enable_ground $enable_air $enable_beacons1 $enable_beacons2
  fi

  # Start Multi-Agent nodes
  # ~/marble/scripts/marble map
  # ~/marble/scripts/marble multiagent

  # Run Frontier Exploration
  # ~/marble/scripts/marble explore_frontier

  sleep 10
  roslaunch ~/marble/scripts/rviz.launch &

elif [ "$1" == "stop" ]; then
  if [ "$2" == "explore_frontier" ]; then
    echo "Stopping MARBLE frontier exploration..."
    kill -9 $(pgrep -f 'python2.*subt_guidance_node.py')
    kill -9 $(pgrep -f 'python.*msfm3d')
    kill -9 $(pgrep -f 'msfm3d.*octomap')
    rosnode kill X1/global_planning
    rosnode kill X2/global_planning
    rosnode kill A01/global_planning
    rosnode kill X1/guidance_controller
    rosnode kill X2/guidance_controller
    rosnode kill A01/guidance_controller
    rosnode kill X1/ground_finder
    rosnode kill X2/ground_finder
    rosnode kill X1/nearness_control_node
    rosnode kill X2/nearness_control_node
  elif [ "$2" == "explore_graph" ]; then
    echo "Stopping MARBLE graph exploration..."
    rosnode kill X1/centering
    rosnode kill X1/cmd_vel_switch
    rosnode kill X1/gbm_node
    rosnode kill X1/graph2path
    rosnode kill X1/graphmsg2rviz
    rosnode kill X1/map2graph
    rosnode kill X2/centering
    rosnode kill X2/cmd_vel_switch
    rosnode kill X2/gbm_node
    rosnode kill X2/graph2path
    rosnode kill X2/graphmsg2rviz
    rosnode kill X2/map2graph
    kill -9 $(pgrep -f 'python.*graph.*.py')
    killall -HUP gbm_node
  elif [ "$2" == "map" ]; then
    echo "Stopping Map Merge nodes..."
    rosnode kill Base/octomap_merger
    rosnode kill X1/octomap_merger
    rosnode kill X2/octomap_merger
  elif [ "$2" == "multiagent" ]; then
    echo "Stopping MARBLE Multi-Agent nodes..."
    # kill -9 $(pgrep -f 'python2.*multi_agent.py')
    rosnode kill Base/multi_agent
    rosnode kill X1/multi_agent
    rosnode kill X2/multi_agent
    rosnode kill A01/multi_agent
    if $enable_beacons1; then
      rosnode kill B01/multi_agent
      rosnode kill B02/multi_agent
    fi
    if $enable_beacons2; then
      rosnode kill B03/multi_agent
      rosnode kill B04/multi_agent
      rosnode kill B05/multi_agent
      rosnode kill B06/multi_agent
      rosnode kill B07/multi_agent
      rosnode kill B08/multi_agent
    fi
  elif [ "$2" == "commscheck" ]; then
    echo "Stopping MARBLE Comms Check nodes..."
    kill -9 $(pgrep -f 'python2.*comms_sim_checker.py')

  else
    echo "Stopping MARBLE services..."
    rosnode kill -a
    killall -9 gzserver
    killall -9 rosmaster
    kill -9 $(pgrep -f 'marker_server')
    kill -9 $(pgrep -f 'terrain')
    kill -9 $(pgrep -f 'ros_ign')
    kill -9 $(pgrep -f 'roslaunch')
    kill -9 $(pgrep -f 'ign launch')
    kill -9 $(pgrep -f 'python.*msfm3d')
    kill -9 $(pgrep -f 'msfm3d.*octomap')
    kill -9 $(pgrep -f 'python.*graph.*.py')
    # kill -9 $(pgrep -f 'python2.*multi_agent.py')
    kill -9 $(pgrep -f 'python2.*comms_sim_checker.py')
    kill -9 $(pgrep -f 'python2.*comms_sim_handler.py')
    kill -9 $(pgrep -f 'python2.*world_to_bot_tf.py')
    kill -9 $(pgrep -f 'python2.*subt_true_odom.py')
    kill -9 $(pgrep -f 'python2.*subt_guidance_node.py')
    kill -9 $(pgrep -f 'subt_ros')
    killall -HUP gbm_node
  fi
elif [ "$1" == "explore" ]; then
  echo "Starting MARBLE Frontier exploration..."
  roslaunch ~/marble/scripts/exploration.launch delay:=60
  # rostopic pub /X1/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
  # roslaunch msfm3d msfm3d_ma.launch vehicle:=X1 &
  # python2 ~/marble/scripts/subt_guidance_node.py "X1" "ground" "L2" 1.0 &

  # sleep 30
  # rostopic pub /X2/cmd_vel geometry_msgs/Twist -1 -- '[1, 0, 0]' '[0, 0, 0]'
  # roslaunch msfm3d msfm3d_ma.launch vehicle:=X2 &
  # python2 ~/marble/scripts/subt_guidance_node.py "X2" "ground" "L2" 1.0 &
elif [ "$1" == "explore_air" ]; then
  echo "Starting MARBLE Frontier aerial exploration..."
  voxblox=false
  if [ "$2" == "voxblox" ]; then
    voxblox=true
  fi
  roslaunch ~/marble/scripts/exploration_air.launch vehicle:=A01 voxblox:=$voxblox
elif [ "$1" == "explore_graph" ]; then
  echo "Starting MARBLE Graph exploration..."
  roslaunch graph-planner centering.launch vehicle:=X1 &
  roslaunch graph-planner junction_switch.launch vehicle:=X1 &
  roslaunch graph-planner junction_planner.launch vehicle:=X1 &
  roslaunch graph-planner graph.launch vehicle:=X1 &
  sleep 60
  roslaunch graph-planner centering.launch vehicle:=X2 &
  roslaunch graph-planner junction_switch.launch vehicle:=X2 &
  roslaunch graph-planner junction_planner.launch vehicle:=X2 &
  roslaunch graph-planner graph.launch vehicle:=X2 &

elif [ "$1" == "map" ]; then
  echo "Starting MARBLE Map Merge nodes..."
  roslaunch octomap_merger octomap_merger_sim.launch vehicle:=Base type:=base fullMerge:=true publishMergedPCL:=false publishDiffPCL:=true &
  roslaunch octomap_merger octomap_merger_sim.launch vehicle:=X1 &
  roslaunch octomap_merger octomap_merger_sim.launch vehicle:=X2 resolution:=0.2 &
  if $enable_air; then
    roslaunch octomap_merger octomap_merger_sim.launch vehicle:=A01 &
  fi

elif [ "$1" == "multiagent" ]; then
  enable_ground=true
  if [ "$2" == "air" ]; then
    enable_air=true
  elif [ "$2" == "air_only" ]; then
    enable_ground=false
    enable_air=true
  fi

  echo "Starting MARBLE Multi-Agent nodes..."
  if $simcomms; then
    launch="multi_agent_sim.launch"
    beacons1=""
    beacons2=""
  else
    launch="multi_agent_virtual.launch"
    # beacons1="B01,B02,B03,B04,B05,B06,B07,B08,B09,B10,B11,B12"
    beacons1="B01,B02"
    # beacons2="B13,B14,B15,B16,B17,B18,B19,B20,B21,B22,B23,B24"
    beacons2="B13,B14"
  fi

  if $enable_air; then
    air="potentialNeighbors:=X1,X2,A01"
    roslaunch marble_multi_agent $launch vehicle:=A01 myBeacons:="" stopCheck:=3600 &
  else
    air="potentialNeighbors:=X1,X2"
  fi

  roslaunch marble_multi_agent $launch vehicle:=Base type:=base $air &
  if $enable_ground; then
    roslaunch marble_multi_agent $launch vehicle:=X1 myBeacons:=$beacons1 delayDrop:=False &
    roslaunch marble_multi_agent $launch vehicle:=X2 myBeacons:=$beacons2 &
  fi
  if $enable_beacons1; then
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B01 type:=beacon &
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B02 type:=beacon &
  fi
  if $enable_beacons2; then
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B03 type:=beacon &
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B04 type:=beacon &
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B05 type:=beacon &
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B06 type:=beacon &
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B07 type:=beacon &
    roslaunch marble_multi_agent multi_agent_sim.launch vehicle:=B08 type:=beacon &
  fi

elif [ "$1" == "commscheck" ]; then
  echo "Starting MARBLE Comms Check nodes..."
  python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py Base &
  if $enable_ground || [ "$2" == "true" ]; then
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py X1 &
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py X2 &
  fi
  if $enable_air || [ "$3" == "true" ]; then
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py A01 &
  fi
  if $enable_beacons1 || $enable_beacons2 || [ "$4" == "true" ]; then
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B01 &
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B02 &
  fi
  if $enable_beacons2 || [ "$5" == "true" ]; then
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B03 &
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B04 &
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B05 &
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B06 &
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B07 &
    python2 ~/catkin/src/marble_multi_agent/src/comms_sim_checker.py B08 &
  fi

elif [ "$1" == "lights" ]; then
  echo "Turning on lights..."
  rosservice call /X1/left_headlight/enable "data: true"
  rosservice call /X1/right_headlight/enable "data: true"
  rosservice call /X2/left_headlight/enable "data: true"
  rosservice call /X2/right_headlight/enable "data: true"
else
  echo "Please tell me if you want to start, stop or explore!"
fi