#!/usr/bin/python

""" 
Run_long_script governs the running of long gazebo_ros_tensorflow simulations.

The core functionality lies in: 
  1. parsing the correct arguments at different levels (tensorflow dnn, gazebo environment, ros supervision)
  2. different crash handling when for instance starting gazebo / tensorflow fails

The script is organized in different steps:
1. Parsing arguments saved in a name space
2. launching ROS  and robot related parameters
3. launching tensorflow in machine (docker/singularity/virtualenv) environment
4. launching experiment with potentially autogenerated gazebo world

Exit code:
0) normal exit code
2) tensorflow stopped working
3) communication with logfolder (Opal) is blocked
4) config file is missing

Example usage:
Let behavior arbitration fly with drone through default canyon in singularity environment 1 time while saving images. 
python run_script.py --robot drone_sim --fsm oracle_drone_fsm --world canyon --reuse_default_world -n 1 -ds -p params.yaml

Author: <NAME>

Dependecies: simulation_supervised, pilot, klaas_robots
"""
import rospy
from std_srvs.srv import Empty as Emptyservice
from std_srvs.srv import EmptyRequest # for pausing and unpausing physics engine
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import SetModelState
from gazebo_msgs.srv import SetModelStateRequest
from gazebo_msgs.msg import ModelState


import ast # to parse startingpositions as string to list


import sys, os, os.path
import subprocess, shlex
import shutil
import time
import signal
import argparse
import yaml
import fnmatch
import numpy as np

class bcolors:
  """ Colors to print in terminal with color!
  """
  HEADER = '\033[95m'
  OKBLUE = '\033[94m'
  OKGREEN = '\033[92m'
  WARNING = '\033[93m'
  FAIL = '\033[91m'
  ENDC = '\033[0m'
  BOLD = '\033[1m'
  UNDERLINE = '\033[4m'


# global variables for Popen objects used for terminating sessions
ros_popen = None
python_popen = None
gazebo_popen = None

crash_number = 0
run_number = 0

def myprint(message):
  """
  Output is not captured on computing cluster,
  therefore write it away to logfolder/output as well
  """
  print(message)  
  with open(FLAGS.summary_dir+FLAGS.log_tag+'/output','a') as f:
    f.write(message+'\n')

# Predefined functions.
def load_param_file(location):
  """Load yaml as dict and change to proper string arguments.
  Note that current implementation will by default change both --key True and --key False to --key."""
  yaml_dict={}
  with open(location, 'r') as stream:
    try:
      yaml_dict=yaml.load(stream)
    except yaml.YAMLError as exc:
      myprint(exc)
  yaml_str=""
  for k in yaml_dict.keys():
    if isinstance(yaml_dict[k],bool):
      yaml_str = "{0} --{1}".format(yaml_str, k)
    else:
      yaml_str = "{0} --{1} {2}".format(yaml_str, k, yaml_dict[k])
  return yaml_str

def wait_for_gazebo():
  """gazebo popen is not enough to get gzserver to stop so wait longer..."""
  p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
  p_grep = subprocess.Popen(["grep","gz"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
  myprint("{0}: wait for gazebo".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
  out = p_grep.communicate()[0]
  while "gzserver" in out:
    p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
    p_grep = subprocess.Popen(["grep","gz"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
    out = p_grep.communicate()[0]
    time.sleep(0.2)
  time.sleep(1)  

def wait_for_create_dataset():
  """gazebo popen is not enough to get gzserver to stop so wait longer..."""
  p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
  p_grep = subprocess.Popen(["grep","create_dataset"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
  myprint("{0}: wait for create_dataset".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
  out = p_grep.communicate()[0]
  while "create_dataset" in out:
    p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
    p_grep = subprocess.Popen(["grep","create_dataset"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
    out = p_grep.communicate()[0]
    time.sleep(0.2)

def wait_for_ros_to_start():
  """Ros might take some time to start the first time so wait till its well in the ps -ef"""  
  time.sleep(1)
  p_ps = subprocess.call(["rosparam", "list"], stdout=subprocess.PIPE)
  while p_ps == 1:
    myprint("{0}: wait for ros".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
    time.sleep(1)
    p_ps = subprocess.call(["rosparam", "list"], stdout=subprocess.PIPE)

def kill_popen(process_name, process_popen):
  """Check status, terminate popen and wait for it to stop."""
  myprint("{0}: terminate {1}".format(time.strftime("%Y-%m-%d_%I:%M:%S"), process_name))
  if process_popen.poll() == None:
    process_popen.terminate()
    process_popen.wait()
  
def kill_combo():
  """kill ros, python and gazebo pids and wait for them to finish"""
  global ros_popen, python_popen, gazebo_popen
  if gazebo_popen: kill_popen('gazebo', gazebo_popen)
  wait_for_gazebo()
  if python_popen: kill_popen('python', python_popen)
  if ros_popen: kill_popen('ros', ros_popen)
  time.sleep(5)




##########################################################################################################################
# STEP 1 Load Parameters

parser = argparse.ArgumentParser(description="""Run_simulation_scripts governs the running of long gazebo_ros_tensorflow simulations.
        The core functionality lies in:
        1. parsing the correct arguments at different levels (tensorflow dnn, gazebo environment, ros supervision)
        2. different crash handling when for instance starting gazebo / tensorflow fails""")

# ==========================
#   General Settings
# ==========================
parser.add_argument("--summary_dir", default='tensorflow/log/', type=str, help="Choose the directory to which tensorflow should save the summaries.")
parser.add_argument("--data_root", default='pilot_data/', type=str, help="Choose the directory to which tensorflow should save the summaries.")
parser.add_argument("--code_root", default='~', type=str, help="Choose the directory to which tensorflow should save the summaries.")
parser.add_argument("-t", "--log_tag", default='testing_on_policy', type=str, help="LOGTAG: tag used to name logfolder.")
parser.add_argument("--data_location", default='', type=str, help="Datalocation is by default the log_tag but than in data_root instead of summary_dir, otherwise FLAG should indicate relative path to data_root.")
parser.add_argument("-n", "--number_of_runs", default=-1, type=int, help="NUMBER_OF_RUNS: define the number of runs the robot will be trained/evaluated. n=1 avoids a hard stop after 5minutes.")
parser.add_argument("-g", "--graphics", action='store_true', help="Add extra nodes for visualization e.g.: Gazebo GUI, control display, depth prediction, ...")
parser.add_argument("-e", "--evaluation", action='store_true',help="This script can launch 2 modes of experiments: training (default) or evaluation.")
parser.add_argument("--evaluate_every", default=10, type=int, help="Evaluate every N runs when training.")
parser.add_argument("--final_evaluation_runs", default=5, type=int, help="Evaluate N times after training is finished..")
parser.add_argument("-ds", "--create_dataset", action='store_true',help="In case of True, sensor data is saved.")
parser.add_argument("--owr", action='store_true',help="Delete dataset if it is already there.")

parser.add_argument("--save_only_success", action='store_true',help="In case of True, sensor data is saved.")
parser.add_argument("--random_seed", type=int, help="If provided, the simulation is seeded (as good as possible).")

# ==========================
#   Robot Settings
# ==========================
parser.add_argument("--robot",default='drone_sim', type=str, help="Specify the robot configuration file: turtle_sim(default), drone_sim, turtle_real, drone_real.")
parser.add_argument("-r", "--recovery", action='store_true',help="Use drone with recovery camera's attached.")

# ==========================
#   Tensorflow Settings
# ==========================
parser.add_argument("-m","--checkpoint_path", type=str, help="Specify the directory of the checkpoint of the earlier trained model.")
parser.add_argument("-pe","--python_environment",default='sing', type=str, help="Define which environment should be loaded in shell when launching tensorlfow. Possibilities: sing, docker, virtualenv.")
parser.add_argument("-pp","--python_project",default='pytorch_pilot/pilot', type=str, help="Define which python module should be started with ~/tenorflow/PROJECT_NAME/main.py: q-learning/pilot, pilot/pilot, ddpg, ....")

# ==========================
#   Environment Settings
# ==========================
parser.add_argument("--auto_go", action='store_true',help="Publish /go signal after few launching gazebo to start experiment automatically")
parser.add_argument("--reuse_default_world", action='store_true',help="reuse the default forest/canyon/sandbox instead of generating them on the fly.")
parser.add_argument("--one_world", action='store_true',help="Reuse one world to train in over and over again.")
parser.add_argument("-w","--world",dest='worlds', action='append', nargs=1, help="Define different worlds: corridor, canyon, forest, sandbox, esat_v1, esat_v2, ... .")
# parser.add_argument("-p","--paramfile",default='eva_params.yaml',type=str, help="Add more parameters to the command loading the DNN in tensorflow ex: eva_params.yaml or params.yaml.")
parser.add_argument("--fsm",default='nn_drone_fsm',type=str, help="Define the fsm loaded from /simsup/config/fsm: nn_turtle_fsm, console_fsm, console_nn_db_turtle_fsm, ...")

parser.add_argument("--x_pos",default=999,type=float, help="Specify x position.")
parser.add_argument("--x_var",default=0,type=float, help="Specify variation in x position.")
parser.add_argument("--y_pos",default=999,type=float, help="Specify y position.")
parser.add_argument("--y_var",default=0,type=float, help="Specify variation in y position.")
parser.add_argument("--z_pos",default=999,type=float, help="Specify z position.")
parser.add_argument("--z_var",default=0,type=float, help="Specify variation z position.")
parser.add_argument("--yaw_or",default=999,type=float, help="Specify yaw orientation.")
# parser.add_argument("--yaw_var",default=2*3.14,type=float, help="Specify variation in yaw orientation.")
parser.add_argument("--yaw_var",default=0,type=float, help="Specify variation in yaw orientation.")

FLAGS, others = parser.parse_known_args()
# FLAGS=parser.parse_args()


# get simulation_supervised dir
simulation_supervised_dir=subprocess.check_output(shlex.split("rospack find simulation_supervised"))[:-1]

# 3 main directories have to be defined in order to make it also runnable from a read-only system-installed singularity image.
if FLAGS.summary_dir[0] != '/':  # 1. Tensorflow log directory for saving tensorflow logs and xterm logs
  FLAGS.summary_dir=os.environ['HOME']+'/'+FLAGS.summary_dir
if FLAGS.data_root[0] != '/':  # 2. Pilot_data directory for saving data
  FLAGS.data_root=os.environ['HOME']+'/'+FLAGS.data_root
if FLAGS.code_root == '~': # 3. location for tensorflow code (and also catkin workspace though they are found with rospack)
  #no explicit directory for code is set so try to parse first from environment
  try:
    FLAGS.code_root = os.environ['CODE']
  except KeyError: # in case environment variable is not set, take home dir
    FLAGS.code_root = os.environ['HOME']

if FLAGS.log_tag == 'testing_on_policy':
  if os.path.isdir(FLAGS.summary_dir+FLAGS.log_tag): shutil.rmtree(FLAGS.summary_dir+FLAGS.log_tag)    
  if os.path.isdir(FLAGS.data_root+FLAGS.log_tag): shutil.rmtree(FLAGS.data_root+FLAGS.log_tag)


# add default values to be able to operate
if FLAGS.worlds == None : FLAGS.worlds=['canyon']
else: #worlds are appended in a nested list... so get them out.
  worlds=[]
  for w in FLAGS.worlds: worlds.append(w[0])
  FLAGS.worlds = worlds[:]

# FLAGS.params=load_param_file(FLAGS.paramfile) if FLAGS.paramfile else ""
FLAGS.params=others[:]

if FLAGS.random_seed: 
  np.random.seed(FLAGS.random_seed)
  FLAGS.params.append('--random_seed '+str(FLAGS.random_seed))

# check if robot configuration exists is there:
if not os.path.isfile(simulation_supervised_dir+'/config/robot/'+FLAGS.robot+'.yaml'):
  myprint("Could not find robot configuration for {}".format(w[0]))
  sys.exit(4)

# try to extract condor host
try:
  FLAGS.condor_host=subprocess.check_output(shlex.split("cat $_CONDOR_JOB_AD | grep RemoteHost | head -1 | cut -d '=' -f 2 | cut -d '@' -f 2 | cut -d '.' -f 1)"))  
except:
  FLAGS.condor_host='unknown_host'

# Clear log folder if desired
if FLAGS.owr and os.path.isdir("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag)):
  shutil.rmtree("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag))

# Create main log folder if necessary
if not os.path.isdir("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag)):
  os.makedirs("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag))
else:
  # Load last position to start from if lastposition is file log folder already existed
  if os.path.isfile("{0}{1}/last_position.txt".format(FLAGS.summary_dir, FLAGS.log_tag)):
    try:
      with open("{0}{1}/last_position.txt".format(FLAGS.summary_dir, FLAGS.log_tag),'r') as f:
        last_position=f.readlines()
        FLAGS.x_pos,FLAGS.y_pos,FLAGS.z_pos,FLAGS.yaw_or= [ float(x) for x in last_position[-1].strip().split(',')]
        myprint("[run_script] obtained last position as {0} {1} {2} {3}".format(FLAGS.x_pos,FLAGS.y_pos,FLAGS.z_pos,FLAGS.yaw_or))
    except:
      myprint("[run_script] failed to obtain last position from {0}{1}/last_position.txt".format(FLAGS.summary_dir, FLAGS.log_tag))

# in case of data_creation, make data_location in ~/pilot_data
if FLAGS.create_dataset: 
  if FLAGS.data_location == "":
    FLAGS.data_location = "{0}{1}".format(FLAGS.data_root, FLAGS.log_tag)
  else:
    FLAGS.data_location = "{0}{1}".format(FLAGS.data_root, FLAGS.data_location)
  if os.path.isdir(FLAGS.data_location) and (FLAGS.number_of_runs == 1 or FLAGS.owr):
    shutil.rmtree(FLAGS.data_location)
  if not os.path.isdir(FLAGS.data_location):
    os.makedirs(FLAGS.data_location)
  else:
    # check number of items already recorded
    if len(os.listdir(FLAGS.data_location)) >= 1:
      # in case there is already data recorded, parse the number of runs and continue from there
      last_run=sorted([d for d in os.listdir(FLAGS.data_location) if os.path.isdir("{0}/{1}".format(FLAGS.data_location,d))])[-1]
      run_number=int(last_run.split('_')[0]) +1 #assuming number occurs at first 5 digits xxxxx_name_of_data
      myprint("Found data from previous run so adjusted run_number to {}".format(run_number))

# display and save all settings
myprint("\nSettings:")
for f in sorted(FLAGS.__dict__): myprint("{0}: {1}".format( f, FLAGS.__dict__[f]))

with open("{0}{1}/run_conf".format(FLAGS.summary_dir, FLAGS.log_tag),'w') as c:
  c.write("Settings of Run_simulation_scripts:\n\n")
  for f in FLAGS.__dict__: c.write("{0}: {1}\n".format(f, FLAGS.__dict__[f]))


##########################################################################################################################
# STEP 2 Start ROS with ROBOT specific parameters

# ensure location for logging the xterm outputs exists.
ros_xterm_log_dir="{0}{1}/xterm_ros".format(FLAGS.summary_dir,FLAGS.log_tag)
if not os.path.isdir(ros_xterm_log_dir): os.makedirs(ros_xterm_log_dir)

def start_ros():
  """Start ros core with robot parameters loaded"""
  global ros_popen
  command="roslaunch simulation_supervised load_params.launch robot_config:={0}.yaml {1}".format(FLAGS.robot, 'random_seed:='+str(FLAGS.random_seed) if FLAGS.random_seed else '')
  if os.path.isfile(simulation_supervised_dir+'/config/environment/'+worlds[0]+'.yaml'):
    command="{0} world_config:={1}".format(command, simulation_supervised_dir+'/config/environment/'+worlds[0]+'.yaml')
  xterm_log_file='{0}/xterm_ros_{1}.txt'.format(ros_xterm_log_dir,time.strftime("%Y-%m-%d_%I%M"))
  if os.path.isfile(xterm_log_file): os.remove(xterm_log_file)
  args = shlex.split("xterm -iconic -l -lf {0} -hold -e {1}".format(xterm_log_file,command))
  ros_popen = subprocess.Popen(args)
  pid_ros = ros_popen.pid
  myprint("{0}: start_ros pid {1}\n".format(time.strftime("%Y-%m-%d_%I:%M:%S"),pid_ros))
  wait_for_ros_to_start()
  rospy.set_param('evaluate_every',FLAGS.evaluate_every if not FLAGS.evaluation else 1)  
  rospy.set_param('recovery',FLAGS.recovery)  
start_ros()

##########################################################################################################################
# STEP 3 Start tensorflow

python_xterm_log_dir="{0}{1}/xterm_python".format(FLAGS.summary_dir,FLAGS.log_tag)
if not os.path.isdir(python_xterm_log_dir): os.makedirs(python_xterm_log_dir)
  
def start_python():
  """Function that initializes python code."""
  # delete default test folder
  # if logdir already exists probably condor job is just restarted somewhere so use last saved q in case of training
  global python_popen
  
  # Add parameters
  FLAGS.log_folder = "{0}{1}".format(FLAGS.summary_dir,FLAGS.log_tag)
  FLAGS.params.append("--log_tag {0}".format(FLAGS.log_tag))
  if not '--on_policy' in FLAGS.params: FLAGS.params.append("--on_policy")
  if FLAGS.checkpoint_path: FLAGS.params.append("--checkpoint_path {0}".format(FLAGS.checkpoint_path))
  
  # Create command
  params=""
  for p in FLAGS.params: params="{0} {1}".format(params,p)
  command="{0}/scripts/launch_python/{1}.sh {2}/tensorflow/{3}/main.py {4}".format(simulation_supervised_dir,
                                                                                FLAGS.python_environment,
                                                                                FLAGS.code_root,
                                                                                FLAGS.python_project,
                                                                                params)
  myprint("Tensorflow command:\n {}".format(command))
  xterm_log_file='{0}/xterm_python_{1}.txt'.format(python_xterm_log_dir,time.strftime("%Y-%m-%d_%I%M"))
  if os.path.isfile(xterm_log_file): os.remove(xterm_log_file)
  args = shlex.split("xterm -l -lf {0} -hold -e {1}".format(xterm_log_file, command))
  # Execute command
  python_popen = subprocess.Popen(args)
  pid_python = python_popen.pid
  myprint("{0}: start_python pid {1} \n\n".format(time.strftime("%Y-%m-%d_%I:%M:%S"),pid_python))
  # Wait for creation of tensorflow log file to know the python node is running
  start_time = time.time()

  wait_time=10
  if os.path.isfile(FLAGS.log_folder+'/nn_ready'):
    prev_stat_nn_ready=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
    while prev_stat_nn_ready == subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready')):
      if time.time()-start_time > wait_time*60:
        myprint("{0}: Waited for {3}minutes on nn_ready in {2} to start, seems like tensorflow has crashed on {1} so exit with error code 2.".format(time.strftime("%Y-%m-%d_%I:%M"), FLAGS.condor_host, FLAGS.log_folder, wait_time))
        kill_combo()
        sys.exit(2)
      time.sleep(1)
  else:
    while(not os.path.isfile(FLAGS.log_folder+'/nn_ready')):
      time.sleep(1)
      if time.time()-start_time > wait_time*60:
        myprint("{0}: Waited for {3}minutes on nn_ready in {2} to start, seems like tensorflow has crashed on {1} so exit with error code 2.".format(time.strftime("%Y-%m-%d_%I:%M"), FLAGS.condor_host, FLAGS.log_folder, wait_time))
        kill_combo()
        sys.exit(2)

start_python()

myprint("[runscript] set recovery to {0}".format(rospy.get_param('recovery')))

##########################################################################################################################
# STEP 4 Start gazebo environment

def create_environment(run_number, world_name):
  """Call correct python script for generating potentially new environment.
  Returns a string with arguments for the launch file to be concatenated to the launch command.
  """
  # generate world if it is possible and allowed, this also changes the loaded world file location from the default simsup_demo/worlds to log_folder
  world_file=''
  world_config=''
  background=''
  # don't create a new world if one_world is on
  if FLAGS.one_world and run_number > 0: return ''
  
  if world_name in ['canyon', 'forest', 'sandbox'] and not FLAGS.reuse_default_world:
    generator_file="{0}/python/generators/{1}_generator.py".format(subprocess.check_output(shlex.split("rospack find simulation_supervised_tools"))[:-1],world_name)
    subprocess.Popen(shlex.split("python "+generator_file+" "+FLAGS.log_folder)).wait()
    background=FLAGS.log_folder+'/'+world_name+'.png'
    world_file=FLAGS.log_folder+'/'+world_name+'.world'
  elif world_name in ['canyon', 'corridor', 'different_corridor'] and FLAGS.reuse_default_world:
    # reuse default 10 evaluation canyons or corridors
    world_file='{0}/../simulation_supervised_demo/worlds/{2}_evaluation/{1:05d}_{2}.world'.format(simulation_supervised_dir,run_number%10, world_name)
    background='{0}/../simulation_supervised_demo/worlds/{2}_evaluation/{1:05d}_{2}.png'.format(simulation_supervised_dir,run_number%10, world_name)
    if 'corridor' in world_name:
      command="{0} world_config:={1}/config/environment/{2:05d}_{3}.yaml".format(command, simulation_supervised_dir,run_number%10, world_name)
  elif world_name in ['corridor'] and not FLAGS.reuse_default_world:
    generator_file="{0}/python/generators/world_generator.py".format(subprocess.check_output(shlex.split("rospack find simulation_supervised_tools"))[:-1])
    generator_command="python "+generator_file+" --output_dir "+FLAGS.log_folder+" --output_file "+world_name+"_"+str(run_number)
    for p in others: generator_command="{0} {1}".format(generator_command, p)
    print("[runscript] Generate command: {0}".format(generator_command))
    return_val=subprocess.call(shlex.split(generator_command))
    if return_val != 0:
      kill_combo()
      myprint("Failed to create env {0}, return value: {1}".format(world_name, return_val))
      sys.exit(2)
    world_file=FLAGS.log_folder+'/'+world_name+"_"+str(run_number)+'.world'
    world_config=FLAGS.log_folder+'/'+world_name+"_"+str(run_number)+'.yaml'  

  arguments='world_name:='+world_name
  for arg in ["world_file", "world_config", "background"]:
    if len(eval(arg)) != 0:  arguments=arguments+" "+arg+":="+eval(arg)
  return arguments
  
def sample_new_position(starting_positions=[]):
  """ Parse a new x,y,z,yaw(quaternion) pose for the robot given the world name and current robot
  returns positions: x, y, z and orientation yaw in quaternion (1 ~ +90)
  """
  # default with arguments
  x, y, z, yaw = 0,0,0,0
  if len(starting_positions) != 0:
    pos = starting_positions[np.random.choice(range(len(starting_positions)))]
    if len(pos) == 2:
      x, y = pos
    elif len(pos) == 3:
      x, y, yaw = pos
    elif len(pos) == 4:
      x, y, z, yaw = pos
    else:
      myprint("[run_script] failed to parse starting_position {0}".format(pos))
  
  # overwrite sampled starting positions if they were manually set
  if FLAGS.x_pos != 999: x=FLAGS.x_pos
  if FLAGS.y_pos != 999: y=FLAGS.y_pos
  if FLAGS.z_pos != 999: z=FLAGS.z_pos
  if FLAGS.yaw_or != 999: yaw=FLAGS.yaw_or
  
  # add some variation
  x += np.random.uniform(-FLAGS.x_var,FLAGS.x_var)
  y += np.random.uniform(-FLAGS.y_var,FLAGS.y_var)
  z += np.random.uniform(-FLAGS.z_var,FLAGS.z_var)
  yaw += np.random.uniform(-FLAGS.yaw_var,FLAGS.yaw_var)

  return x, y, z, yaw 

# ensure location for logging the xterm outputs exists.
gazebo_xterm_log_dir="{0}{1}/xterm_gazebo".format(FLAGS.summary_dir,FLAGS.log_tag)
if not os.path.isdir(gazebo_xterm_log_dir): os.makedirs(gazebo_xterm_log_dir)

# Some local variables for running different simulations
prev_environment_arguments=''
reset_gazebo_service=rospy.ServiceProxy('/gazebo/reset_simulation',Emptyservice)
model_state_gazebo_service=rospy.ServiceProxy('/gazebo/set_model_state',SetModelState)
unpause_physics_client=rospy.ServiceProxy('/gazebo/unpause_physics',Emptyservice)

gazebo_popen=None
prev_stat_nn_log=''
prev_stat_fsm_log=''
fsm_file = FLAGS.log_folder+'/fsm_log'
if not os.path.isfile(fsm_file):
  with open(fsm_file,'a') as f: 
    f.write('{0}: {1}\n'.format(time.strftime("%Y-%m-%d_%I-%M-%S"), FLAGS.log_folder))


crashed=False 
while (run_number < FLAGS.number_of_runs) or FLAGS.number_of_runs==-1:
  
  ######################################
  # 4.1 Prepare Run
  world_name = FLAGS.worlds[run_number%len(FLAGS.worlds)]

  # save current status of NN nn_ready to compare afterwards
  if os.path.isfile(FLAGS.log_folder+'/nn_ready'):
    prev_stat_nn_log=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
  else: # we have last communication with our log folder so exit with code 2
    myprint("{2}: lost communication with our log folder {0} on host {1} so exit with code 3.".format(FLAGS.log_folder, FLAGS.condor_host, time.strftime("%Y-%m-%d_%I:%M:%S")))
    kill_combo()
    sys.exit(3)

  # clean up gazebo ros folder every now and then
  if run_number%50 == 0 : shutil.rmtree("{0}/.gazebo/log".format(os.environ['HOME']),ignore_errors=True)
  
  evaluate=((run_number%FLAGS.evaluate_every) == 0 and run_number != 0 and FLAGS.evaluate_every != -1) or FLAGS.evaluation
  
  # if evaluate:
  #   rospy.set_param('max_duration', 120)
  # else:
  #   rospy.set_param('max_duration', 5)

  new_environment_arguments=create_environment(run_number, world_name)
  
  ######################################
  # 4.2 Create environment and perform next run

  if rospy.has_param('/starting_positions'):
    starting_positions = rospy.get_param('starting_positions')
    if isinstance(starting_positions,str):
      starting_positions=ast.literal_eval(starting_positions)
  else:
    starting_positions = []

  if (new_environment_arguments == prev_environment_arguments or len(new_environment_arguments) == 0) and not crashed and gazebo_popen != None:
    # 4.2.1 Reset environment for next run if possible
    # 4.2.1a Ensure correct settings
    rospy.set_param('/evaluate',evaluate)
    
    # 4.2.1b Reset environment ==> causes gt_node to freeze for more than a minute...
    # reset_gazebo_service(EmptyRequest())
    
    # 4.2.1c Change position of drone according to new selected starting position
    pose=Pose()
    pose.position.x, pose.position.y, starting_height, yaw = sample_new_position(starting_positions)
    # pose.position.x, pose.position.y, starting_height, yaw=0,0,1,0
    
    myprint("[run_script]: x: {0}, y: {1}, z: {2}, yaw:{3}".format(pose.position.x, pose.position.y, starting_height, yaw))
    # some yaw to quaternion re-orientation code:
    pose.orientation.z=np.sin(yaw)
    pose.orientation.w=np.cos(yaw)
    pose.position.z = 0.1
    model_state = ModelState()
    model_state.model_name = 'quadrotor' if FLAGS.robot.startswith('drone') else 'turtlebot3_burger'
    model_state.pose=pose
    state_request = SetModelStateRequest()
    state_request.model_state = model_state
    retvals = model_state_gazebo_service(state_request)
    rospy.set_param('starting_height', starting_height)
    
    myprint("Changed pose with return values: {0}".format(retvals))
    time.sleep(5) # HAS to be 5 otherwise '/overtake' and '/ready' overlap resulting in empty images in gt_listener
    unpause_physics_client(EmptyRequest())

  else:
    # 4.2.2 Launch Gazebo again
    # 4.2.2a Ensure previous Gazebo is not running anymore
    if gazebo_popen!=None: 
      kill_popen('gazebo', gazebo_popen)
      wait_for_gazebo()

    prev_environment_arguments = new_environment_arguments
    
    # 4.2.2b Build command with correct settings
    # remove if saving location already exists (probably due to crash previously)
    if FLAGS.create_dataset:
      data_location="{0}/{1:05d}_{2}".format(FLAGS.data_location,run_number,world_name)
      if os.path.isdir(data_location): shutil.rmtree(data_location)
      os.makedirs(data_location)
      new_environment_arguments+=" save_images:=true"
      new_environment_arguments+=" data_location:={0}".format(data_location)
      if 'world_file' in new_environment_arguments:
        world_file=[a for a in new_environment_arguments.split(' ') if 'world_file' in a][0].split(':=')[1]
        myprint("[runscript] world_file {0}".format(world_file))
        shutil.copyfile(world_file, data_location+'/'+os.path.basename(world_file))

    x,y,z,yaw=sample_new_position(starting_positions)
    # x,y,z,yaw=-54, -4, 1, -3.14

    command="roslaunch simulation_supervised_demo {0}.launch fsm_config:={1} log_folder:={2} evaluate:={3} {4} graphics:={5} x:={6} y:={7} Yspawned:={9} starting_height:={8} {10}".format(FLAGS.robot,
            FLAGS.fsm,
            FLAGS.log_folder,
            'true' if evaluate else 'false',
            new_environment_arguments,
            'true' if FLAGS.graphics else 'false',
            x,y,z,yaw,
            'random_seed:='+str(FLAGS.random_seed) if FLAGS.random_seed else '')
    # 4.2.2c Launch command
    # Execute command
    myprint( "gazebo_command: {0}".format(command))
    xterm_log_file='{0}/xterm_gazebo_{1}.txt'.format(gazebo_xterm_log_dir,time.strftime("%Y-%m-%d_%I-%M-%S"))
    args = shlex.split("xterm -iconic -l -lf {0} -hold -e {1}".format(xterm_log_file,command))
    gazebo_popen = subprocess.Popen(args)
    pid_gazebo = gazebo_popen.pid


  ######################################
  # 4.3 Wait for run to finish
  # on this moment the run is not crashed (yet).
  crashed=False 
  crash_checked=False
  
  #print starting positions for visualizing later.
  with open(FLAGS.log_folder+'/starting_positions.txt','a') as f:
    f.write('{0}, {1}, {2}\n'.format(x,y,yaw))

  prev_stat_fsm_log=subprocess.check_output(shlex.split("stat -c %Y "+fsm_file))
  time.sleep(0.1)
  myprint("\n{0}: started run {1} of the {2} in {4} {3} {5}".format(time.strftime("%Y-%m-%d_%I:%M:%S"),
                                                          run_number+1, 
                                                          FLAGS.number_of_runs,
                                                          world_name,
                                                          bcolors.OKBLUE,
                                                          bcolors.ENDC))

  start_time=time.time()
  time_spend=0
  # while fsm_file has not been updated, wait...
  while prev_stat_fsm_log == subprocess.check_output(shlex.split("stat -c %Y "+fsm_file)):
    # Check on job suspension: 
    # if between last update and now has been more than 30 seconds (should be less than 0.1s)
    if time.time() - start_time - time_spend > 30:
      myprint("{0}: Job got suspended.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
      time.sleep(30) #wait for big tick to update
      start_time=time.time()
    else:
      time_spend=time.time() - start_time

    # automatically start with /go after 10s
    if FLAGS.auto_go:
      if 10.05 <= time_spend<10.15: 
        go_popen=subprocess.Popen(shlex.split("rostopic pub /go std_msgs/Empty"))
      elif 11.15 <= time_spend < 11.25 and go_popen.poll()==None:
        kill_popen('go', go_popen)
    
    # if False:
    # if time_spend > 60*10 and FLAGS.number_of_runs != 1: #don't interupt if this is a single run
    if time_spend > 5 and not crash_checked:
      crash_checked = True
      # check for crash
      with open(xterm_log_file, 'r') as f:
        for l in f.readlines():
          if 'process has died' in l:
            myprint("[run_script] {0}: found gz crash in {1}: {2}.".format(time.strftime("%Y-%m-%d_%I:%M:%S"), os.path.basename(xterm_log_file),l[:50]))
            crashed=True
            crash_number+=1
      if crashed:
        if crash_number < 10: #after 20 crashes its maybe time to restart everything
          kill_popen('gazebo', gazebo_popen)
        else:
          myprint("{0}: crashed for 10the time so restart everything.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
          kill_combo()
          start_ros()
          start_python()
          crash_number = 0
        break # get out of this loop
    time.sleep(0.1)

  ######################################
  # 4.4 Clean up run
  # 4.4.1 Wait for NN framework if it is running
  if not crashed and 'nn' in FLAGS.fsm:
    # wait for nn_ready and stop in case of no tensorflow communication
    if os.path.isfile(FLAGS.log_folder+'/nn_ready'):
      current_stat=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
      start_time=time.time()
      myprint("{0}: waiting for nn_ready.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
      while current_stat == prev_stat_nn_log:
        current_stat=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
        time.sleep(1)
        if time.time()-start_time > 8*60:
          myprint("{0}: waited for 8minutes on nn_ready to finish training so something went wrong on {1} exit with code 2.".format(time.strftime("%Y-%m-%d_%I:%M:%S"), FLAGS.condor_host))
          kill_combo()
          sys.exit(2)
    else:
      myprint("{2}: we have lost communication with our log folder {0} on host {1} so exit with code 3.".format(FLAGS.log_folder, FLAGS.condor_host, time.strftime("%Y-%m-%d_%I:%M:%S")))
      kill_combo()
      sys.exit(3)
  if not crashed:
    message = open(fsm_file,'r').readlines()[-1].strip()
    myprint("{0}: ended run {1} with {3}{2}{4}".format(time.strftime("%Y-%m-%d_%I:%M:%S"), run_number+1, message, bcolors.OKGREEN if 'success' in message else bcolors.FAIL, bcolors.ENDC))
    # increment also in case of crash as drone has zero turning speed:
    run_number+=1
    if message == 'FINISHED': # make this the final run for evaluation
      FLAGS.number_of_runs=run_number+FLAGS.final_evaluation_runs
      FLAGS.evaluation=True

      # run_number = FLAGS.number_of_runs-1
  time.sleep(3) 
  # extra second needed to save image in gt_listener

# after all required runs are finished
kill_combo()
myprint("\n{0}: done.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))


#!/usr/bin/python

""" 
Run_long_script governs the running of long gazebo_ros_tensorflow simulations.

The core functionality lies in: 
  1. parsing the correct arguments at different levels (tensorflow dnn, gazebo environment, ros supervision)
  2. different crash handling when for instance starting gazebo / tensorflow fails

The script is organized in different steps:
1. Parsing arguments saved in a name space
2. launching ROS  and robot related parameters
3. launching tensorflow in machine (docker/singularity/virtualenv) environment
4. launching experiment with potentially autogenerated gazebo world

Exit code:
0) normal exit code
2) tensorflow stopped working
3) communication with logfolder (Opal) is blocked
4) config file is missing

Example usage:
Let behavior arbitration fly with drone through default canyon in singularity environment 1 time while saving images. 
python run_script.py --robot drone_sim --fsm oracle_drone_fsm --world canyon --reuse_default_world -n 1 -ds -p params.yaml

Author: <NAME>

Dependecies: simulation_supervised, pilot, klaas_robots
"""
import rospy
from std_srvs.srv import Empty as Emptyservice
from std_srvs.srv import EmptyRequest # for pausing and unpausing physics engine
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import SetModelState
from gazebo_msgs.srv import SetModelStateRequest
from gazebo_msgs.msg import ModelState


import ast # to parse startingpositions as string to list


import sys, os, os.path
import subprocess, shlex
import shutil
import time
import signal
import argparse
import yaml
import fnmatch
import numpy as np

class bcolors:
  """ Colors to print in terminal with color!
  """
  HEADER = '\033[95m'
  OKBLUE = '\033[94m'
  OKGREEN = '\033[92m'
  WARNING = '\033[93m'
  FAIL = '\033[91m'
  ENDC = '\033[0m'
  BOLD = '\033[1m'
  UNDERLINE = '\033[4m'


# global variables for Popen objects used for terminating sessions
ros_popen = None
python_popen = None
gazebo_popen = None

crash_number = 0
run_number = 0

def myprint(message):
  """
  Output is not captured on computing cluster,
  therefore write it away to logfolder/output as well
  """
  print(message)  
  with open(FLAGS.summary_dir+FLAGS.log_tag+'/output','a') as f:
    f.write(message+'\n')

# Predefined functions.
def load_param_file(location):
  """Load yaml as dict and change to proper string arguments.
  Note that current implementation will by default change both --key True and --key False to --key."""
  yaml_dict={}
  with open(location, 'r') as stream:
    try:
      yaml_dict=yaml.load(stream)
    except yaml.YAMLError as exc:
      myprint(exc)
  yaml_str=""
  for k in yaml_dict.keys():
    if isinstance(yaml_dict[k],bool):
      yaml_str = "{0} --{1}".format(yaml_str, k)
    else:
      yaml_str = "{0} --{1} {2}".format(yaml_str, k, yaml_dict[k])
  return yaml_str

def wait_for_gazebo():
  """gazebo popen is not enough to get gzserver to stop so wait longer..."""
  p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
  p_grep = subprocess.Popen(["grep","gz"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
  myprint("{0}: wait for gazebo".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
  out = p_grep.communicate()[0]
  while "gzserver" in out:
    p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
    p_grep = subprocess.Popen(["grep","gz"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
    out = p_grep.communicate()[0]
    time.sleep(0.2)
  time.sleep(1)  

def wait_for_create_dataset():
  """gazebo popen is not enough to get gzserver to stop so wait longer..."""
  p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
  p_grep = subprocess.Popen(["grep","create_dataset"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
  myprint("{0}: wait for create_dataset".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
  out = p_grep.communicate()[0]
  while "create_dataset" in out:
    p_ps = subprocess.Popen(["ps", "-ef"], stdout=subprocess.PIPE)
    p_grep = subprocess.Popen(["grep","create_dataset"],stdin=p_ps.stdout, stdout=subprocess.PIPE)
    out = p_grep.communicate()[0]
    time.sleep(0.2)

def wait_for_ros_to_start():
  """Ros might take some time to start the first time so wait till its well in the ps -ef"""  
  time.sleep(1)
  p_ps = subprocess.call(["rosparam", "list"], stdout=subprocess.PIPE)
  while p_ps == 1:
    myprint("{0}: wait for ros".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
    time.sleep(1)
    p_ps = subprocess.call(["rosparam", "list"], stdout=subprocess.PIPE)

def kill_popen(process_name, process_popen):
  """Check status, terminate popen and wait for it to stop."""
  myprint("{0}: terminate {1}".format(time.strftime("%Y-%m-%d_%I:%M:%S"), process_name))
  if process_popen.poll() == None:
    process_popen.terminate()
    process_popen.wait()
  
def kill_combo():
  """kill ros, python and gazebo pids and wait for them to finish"""
  global ros_popen, python_popen, gazebo_popen
  if gazebo_popen: kill_popen('gazebo', gazebo_popen)
  wait_for_gazebo()
  if python_popen: kill_popen('python', python_popen)
  if ros_popen: kill_popen('ros', ros_popen)
  time.sleep(5)




##########################################################################################################################
# STEP 1 Load Parameters

parser = argparse.ArgumentParser(description="""Run_simulation_scripts governs the running of long gazebo_ros_tensorflow simulations.
        The core functionality lies in:
        1. parsing the correct arguments at different levels (tensorflow dnn, gazebo environment, ros supervision)
        2. different crash handling when for instance starting gazebo / tensorflow fails""")

# ==========================
#   General Settings
# ==========================
parser.add_argument("--summary_dir", default='tensorflow/log/', type=str, help="Choose the directory to which tensorflow should save the summaries.")
parser.add_argument("--data_root", default='pilot_data/', type=str, help="Choose the directory to which tensorflow should save the summaries.")
parser.add_argument("--code_root", default='~', type=str, help="Choose the directory to which tensorflow should save the summaries.")
parser.add_argument("-t", "--log_tag", default='testing_on_policy', type=str, help="LOGTAG: tag used to name logfolder.")
parser.add_argument("--data_location", default='', type=str, help="Datalocation is by default the log_tag but than in data_root instead of summary_dir, otherwise FLAG should indicate relative path to data_root.")
parser.add_argument("-n", "--number_of_runs", default=-1, type=int, help="NUMBER_OF_RUNS: define the number of runs the robot will be trained/evaluated. n=1 avoids a hard stop after 5minutes.")
parser.add_argument("-g", "--graphics", action='store_true', help="Add extra nodes for visualization e.g.: Gazebo GUI, control display, depth prediction, ...")
parser.add_argument("-e", "--evaluation", action='store_true',help="This script can launch 2 modes of experiments: training (default) or evaluation.")
parser.add_argument("--evaluate_every", default=10, type=int, help="Evaluate every N runs when training.")
parser.add_argument("--final_evaluation_runs", default=5, type=int, help="Evaluate N times after training is finished..")
parser.add_argument("-ds", "--create_dataset", action='store_true',help="In case of True, sensor data is saved.")
parser.add_argument("--owr", action='store_true',help="Delete dataset if it is already there.")

parser.add_argument("--save_only_success", action='store_true',help="In case of True, sensor data is saved.")
parser.add_argument("--random_seed", type=int, help="If provided, the simulation is seeded (as good as possible).")

# ==========================
#   Robot Settings
# ==========================
parser.add_argument("--robot",default='drone_sim', type=str, help="Specify the robot configuration file: turtle_sim(default), drone_sim, turtle_real, drone_real.")
parser.add_argument("-r", "--recovery", action='store_true',help="Use drone with recovery camera's attached.")

# ==========================
#   Tensorflow Settings
# ==========================
parser.add_argument("-m","--checkpoint_path", type=str, help="Specify the directory of the checkpoint of the earlier trained model.")
parser.add_argument("-pe","--python_environment",default='sing', type=str, help="Define which environment should be loaded in shell when launching tensorlfow. Possibilities: sing, docker, virtualenv.")
parser.add_argument("-pp","--python_project",default='pytorch_pilot/pilot', type=str, help="Define which python module should be started with ~/tenorflow/PROJECT_NAME/main.py: q-learning/pilot, pilot/pilot, ddpg, ....")

# ==========================
#   Environment Settings
# ==========================
parser.add_argument("--auto_go", action='store_true',help="Publish /go signal after few launching gazebo to start experiment automatically")
parser.add_argument("--reuse_default_world", action='store_true',help="reuse the default forest/canyon/sandbox instead of generating them on the fly.")
parser.add_argument("--one_world", action='store_true',help="Reuse one world to train in over and over again.")
parser.add_argument("-w","--world",dest='worlds', action='append', nargs=1, help="Define different worlds: corridor, canyon, forest, sandbox, esat_v1, esat_v2, ... .")
# parser.add_argument("-p","--paramfile",default='eva_params.yaml',type=str, help="Add more parameters to the command loading the DNN in tensorflow ex: eva_params.yaml or params.yaml.")
parser.add_argument("--fsm",default='nn_drone_fsm',type=str, help="Define the fsm loaded from /simsup/config/fsm: nn_turtle_fsm, console_fsm, console_nn_db_turtle_fsm, ...")

parser.add_argument("--x_pos",default=999,type=float, help="Specify x position.")
parser.add_argument("--x_var",default=0,type=float, help="Specify variation in x position.")
parser.add_argument("--y_pos",default=999,type=float, help="Specify y position.")
parser.add_argument("--y_var",default=0,type=float, help="Specify variation in y position.")
parser.add_argument("--z_pos",default=999,type=float, help="Specify z position.")
parser.add_argument("--z_var",default=0,type=float, help="Specify variation z position.")
parser.add_argument("--yaw_or",default=999,type=float, help="Specify yaw orientation.")
# parser.add_argument("--yaw_var",default=2*3.14,type=float, help="Specify variation in yaw orientation.")
parser.add_argument("--yaw_var",default=0,type=float, help="Specify variation in yaw orientation.")

FLAGS, others = parser.parse_known_args()
# FLAGS=parser.parse_args()


# get simulation_supervised dir
simulation_supervised_dir=subprocess.check_output(shlex.split("rospack find simulation_supervised"))[:-1]

# 3 main directories have to be defined in order to make it also runnable from a read-only system-installed singularity image.
if FLAGS.summary_dir[0] != '/':  # 1. Tensorflow log directory for saving tensorflow logs and xterm logs
  FLAGS.summary_dir=os.environ['HOME']+'/'+FLAGS.summary_dir
if FLAGS.data_root[0] != '/':  # 2. Pilot_data directory for saving data
  FLAGS.data_root=os.environ['HOME']+'/'+FLAGS.data_root
if FLAGS.code_root == '~': # 3. location for tensorflow code (and also catkin workspace though they are found with rospack)
  #no explicit directory for code is set so try to parse first from environment
  try:
    FLAGS.code_root = os.environ['CODE']
  except KeyError: # in case environment variable is not set, take home dir
    FLAGS.code_root = os.environ['HOME']

if FLAGS.log_tag == 'testing_on_policy':
  if os.path.isdir(FLAGS.summary_dir+FLAGS.log_tag): shutil.rmtree(FLAGS.summary_dir+FLAGS.log_tag)    
  if os.path.isdir(FLAGS.data_root+FLAGS.log_tag): shutil.rmtree(FLAGS.data_root+FLAGS.log_tag)


# add default values to be able to operate
if FLAGS.worlds == None : FLAGS.worlds=['canyon']
else: #worlds are appended in a nested list... so get them out.
  worlds=[]
  for w in FLAGS.worlds: worlds.append(w[0])
  FLAGS.worlds = worlds[:]

# FLAGS.params=load_param_file(FLAGS.paramfile) if FLAGS.paramfile else ""
FLAGS.params=others[:]

if FLAGS.random_seed: 
  np.random.seed(FLAGS.random_seed)
  FLAGS.params.append('--random_seed '+str(FLAGS.random_seed))

# check if robot configuration exists is there:
if not os.path.isfile(simulation_supervised_dir+'/config/robot/'+FLAGS.robot+'.yaml'):
  myprint("Could not find robot configuration for {}".format(w[0]))
  sys.exit(4)

# try to extract condor host
try:
  FLAGS.condor_host=subprocess.check_output(shlex.split("cat $_CONDOR_JOB_AD | grep RemoteHost | head -1 | cut -d '=' -f 2 | cut -d '@' -f 2 | cut -d '.' -f 1)"))  
except:
  FLAGS.condor_host='unknown_host'

# Clear log folder if desired
if FLAGS.owr and os.path.isdir("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag)):
  shutil.rmtree("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag))

# Create main log folder if necessary
if not os.path.isdir("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag)):
  os.makedirs("{0}{1}".format(FLAGS.summary_dir, FLAGS.log_tag))
else:
  # Load last position to start from if lastposition is file log folder already existed
  if os.path.isfile("{0}{1}/last_position.txt".format(FLAGS.summary_dir, FLAGS.log_tag)):
    try:
      with open("{0}{1}/last_position.txt".format(FLAGS.summary_dir, FLAGS.log_tag),'r') as f:
        last_position=f.readlines()
        FLAGS.x_pos,FLAGS.y_pos,FLAGS.z_pos,FLAGS.yaw_or= [ float(x) for x in last_position[-1].strip().split(',')]
        myprint("[run_script] obtained last position as {0} {1} {2} {3}".format(FLAGS.x_pos,FLAGS.y_pos,FLAGS.z_pos,FLAGS.yaw_or))
    except:
      myprint("[run_script] failed to obtain last position from {0}{1}/last_position.txt".format(FLAGS.summary_dir, FLAGS.log_tag))

# in case of data_creation, make data_location in ~/pilot_data
if FLAGS.create_dataset: 
  if FLAGS.data_location == "":
    FLAGS.data_location = "{0}{1}".format(FLAGS.data_root, FLAGS.log_tag)
  else:
    FLAGS.data_location = "{0}{1}".format(FLAGS.data_root, FLAGS.data_location)
  if os.path.isdir(FLAGS.data_location) and (FLAGS.number_of_runs == 1 or FLAGS.owr):
    shutil.rmtree(FLAGS.data_location)
  if not os.path.isdir(FLAGS.data_location):
    os.makedirs(FLAGS.data_location)
  else:
    # check number of items already recorded
    if len(os.listdir(FLAGS.data_location)) >= 1:
      # in case there is already data recorded, parse the number of runs and continue from there
      last_run=sorted([d for d in os.listdir(FLAGS.data_location) if os.path.isdir("{0}/{1}".format(FLAGS.data_location,d))])[-1]
      run_number=int(last_run.split('_')[0]) +1 #assuming number occurs at first 5 digits xxxxx_name_of_data
      myprint("Found data from previous run so adjusted run_number to {}".format(run_number))

# display and save all settings
myprint("\nSettings:")
for f in sorted(FLAGS.__dict__): myprint("{0}: {1}".format( f, FLAGS.__dict__[f]))

with open("{0}{1}/run_conf".format(FLAGS.summary_dir, FLAGS.log_tag),'w') as c:
  c.write("Settings of Run_simulation_scripts:\n\n")
  for f in FLAGS.__dict__: c.write("{0}: {1}\n".format(f, FLAGS.__dict__[f]))


##########################################################################################################################
# STEP 2 Start ROS with ROBOT specific parameters

# ensure location for logging the xterm outputs exists.
ros_xterm_log_dir="{0}{1}/xterm_ros".format(FLAGS.summary_dir,FLAGS.log_tag)
if not os.path.isdir(ros_xterm_log_dir): os.makedirs(ros_xterm_log_dir)

def start_ros():
  """Start ros core with robot parameters loaded"""
  global ros_popen
  command="roslaunch simulation_supervised load_params.launch robot_config:={0}.yaml {1}".format(FLAGS.robot, 'random_seed:='+str(FLAGS.random_seed) if FLAGS.random_seed else '')
  if os.path.isfile(simulation_supervised_dir+'/config/environment/'+worlds[0]+'.yaml'):
    command="{0} world_config:={1}".format(command, simulation_supervised_dir+'/config/environment/'+worlds[0]+'.yaml')
  xterm_log_file='{0}/xterm_ros_{1}.txt'.format(ros_xterm_log_dir,time.strftime("%Y-%m-%d_%I%M"))
  if os.path.isfile(xterm_log_file): os.remove(xterm_log_file)
  args = shlex.split("xterm -iconic -l -lf {0} -hold -e {1}".format(xterm_log_file,command))
  ros_popen = subprocess.Popen(args)
  pid_ros = ros_popen.pid
  myprint("{0}: start_ros pid {1}\n".format(time.strftime("%Y-%m-%d_%I:%M:%S"),pid_ros))
  wait_for_ros_to_start()
  rospy.set_param('evaluate_every',FLAGS.evaluate_every if not FLAGS.evaluation else 1)  
  rospy.set_param('recovery',FLAGS.recovery)  
start_ros()

##########################################################################################################################
# STEP 3 Start tensorflow

python_xterm_log_dir="{0}{1}/xterm_python".format(FLAGS.summary_dir,FLAGS.log_tag)
if not os.path.isdir(python_xterm_log_dir): os.makedirs(python_xterm_log_dir)
  
def start_python():
  """Function that initializes python code."""
  # delete default test folder
  # if logdir already exists probably condor job is just restarted somewhere so use last saved q in case of training
  global python_popen
  
  # Add parameters
  FLAGS.log_folder = "{0}{1}".format(FLAGS.summary_dir,FLAGS.log_tag)
  FLAGS.params.append("--log_tag {0}".format(FLAGS.log_tag))
  if not '--on_policy' in FLAGS.params: FLAGS.params.append("--on_policy")
  if FLAGS.checkpoint_path: FLAGS.params.append("--checkpoint_path {0}".format(FLAGS.checkpoint_path))
  
  # Create command
  params=""
  for p in FLAGS.params: params="{0} {1}".format(params,p)
  command="{0}/scripts/launch_python/{1}.sh {2}/tensorflow/{3}/main.py {4}".format(simulation_supervised_dir,
                                                                                FLAGS.python_environment,
                                                                                FLAGS.code_root,
                                                                                FLAGS.python_project,
                                                                                params)
  myprint("Tensorflow command:\n {}".format(command))
  xterm_log_file='{0}/xterm_python_{1}.txt'.format(python_xterm_log_dir,time.strftime("%Y-%m-%d_%I%M"))
  if os.path.isfile(xterm_log_file): os.remove(xterm_log_file)
  args = shlex.split("xterm -l -lf {0} -hold -e {1}".format(xterm_log_file, command))
  # Execute command
  python_popen = subprocess.Popen(args)
  pid_python = python_popen.pid
  myprint("{0}: start_python pid {1} \n\n".format(time.strftime("%Y-%m-%d_%I:%M:%S"),pid_python))
  # Wait for creation of tensorflow log file to know the python node is running
  start_time = time.time()

  wait_time=10
  if os.path.isfile(FLAGS.log_folder+'/nn_ready'):
    prev_stat_nn_ready=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
    while prev_stat_nn_ready == subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready')):
      if time.time()-start_time > wait_time*60:
        myprint("{0}: Waited for {3}minutes on nn_ready in {2} to start, seems like tensorflow has crashed on {1} so exit with error code 2.".format(time.strftime("%Y-%m-%d_%I:%M"), FLAGS.condor_host, FLAGS.log_folder, wait_time))
        kill_combo()
        sys.exit(2)
      time.sleep(1)
  else:
    while(not os.path.isfile(FLAGS.log_folder+'/nn_ready')):
      time.sleep(1)
      if time.time()-start_time > wait_time*60:
        myprint("{0}: Waited for {3}minutes on nn_ready in {2} to start, seems like tensorflow has crashed on {1} so exit with error code 2.".format(time.strftime("%Y-%m-%d_%I:%M"), FLAGS.condor_host, FLAGS.log_folder, wait_time))
        kill_combo()
        sys.exit(2)

start_python()

myprint("[runscript] set recovery to {0}".format(rospy.get_param('recovery')))

##########################################################################################################################
# STEP 4 Start gazebo environment

def create_environment(run_number, world_name):
  """Call correct python script for generating potentially new environment.
  Returns a string with arguments for the launch file to be concatenated to the launch command.
  """
  # generate world if it is possible and allowed, this also changes the loaded world file location from the default simsup_demo/worlds to log_folder
  world_file=''
  world_config=''
  background=''
  # don't create a new world if one_world is on
  if FLAGS.one_world and run_number > 0: return ''
  
  if world_name in ['canyon', 'forest', 'sandbox'] and not FLAGS.reuse_default_world:
    generator_file="{0}/python/generators/{1}_generator.py".format(subprocess.check_output(shlex.split("rospack find simulation_supervised_tools"))[:-1],world_name)
    subprocess.Popen(shlex.split("python "+generator_file+" "+FLAGS.log_folder)).wait()
    background=FLAGS.log_folder+'/'+world_name+'.png'
    world_file=FLAGS.log_folder+'/'+world_name+'.world'
  elif world_name in ['canyon', 'corridor', 'different_corridor'] and FLAGS.reuse_default_world:
    # reuse default 10 evaluation canyons or corridors
    world_file='{0}/../simulation_supervised_demo/worlds/{2}_evaluation/{1:05d}_{2}.world'.format(simulation_supervised_dir,run_number%10, world_name)
    background='{0}/../simulation_supervised_demo/worlds/{2}_evaluation/{1:05d}_{2}.png'.format(simulation_supervised_dir,run_number%10, world_name)
    if 'corridor' in world_name:
      command="{0} world_config:={1}/config/environment/{2:05d}_{3}.yaml".format(command, simulation_supervised_dir,run_number%10, world_name)
  elif world_name in ['corridor'] and not FLAGS.reuse_default_world:
    generator_file="{0}/python/generators/world_generator.py".format(subprocess.check_output(shlex.split("rospack find simulation_supervised_tools"))[:-1])
    generator_command="python "+generator_file+" --output_dir "+FLAGS.log_folder+" --output_file "+world_name+"_"+str(run_number)
    for p in others: generator_command="{0} {1}".format(generator_command, p)
    print("[runscript] Generate command: {0}".format(generator_command))
    return_val=subprocess.call(shlex.split(generator_command))
    if return_val != 0:
      kill_combo()
      myprint("Failed to create env {0}, return value: {1}".format(world_name, return_val))
      sys.exit(2)
    world_file=FLAGS.log_folder+'/'+world_name+"_"+str(run_number)+'.world'
    world_config=FLAGS.log_folder+'/'+world_name+"_"+str(run_number)+'.yaml'  

  arguments='world_name:='+world_name
  for arg in ["world_file", "world_config", "background"]:
    if len(eval(arg)) != 0:  arguments=arguments+" "+arg+":="+eval(arg)
  return arguments
  
def sample_new_position(starting_positions=[]):
  """ Parse a new x,y,z,yaw(quaternion) pose for the robot given the world name and current robot
  returns positions: x, y, z and orientation yaw in quaternion (1 ~ +90)
  """
  # default with arguments
  x, y, z, yaw = 0,0,0,0
  if len(starting_positions) != 0:
    pos = starting_positions[np.random.choice(range(len(starting_positions)))]
    if len(pos) == 2:
      x, y = pos
    elif len(pos) == 3:
      x, y, yaw = pos
    elif len(pos) == 4:
      x, y, z, yaw = pos
    else:
      myprint("[run_script] failed to parse starting_position {0}".format(pos))
  
  # overwrite sampled starting positions if they were manually set
  if FLAGS.x_pos != 999: x=FLAGS.x_pos
  if FLAGS.y_pos != 999: y=FLAGS.y_pos
  if FLAGS.z_pos != 999: z=FLAGS.z_pos
  if FLAGS.yaw_or != 999: yaw=FLAGS.yaw_or
  
  # add some variation
  x += np.random.uniform(-FLAGS.x_var,FLAGS.x_var)
  y += np.random.uniform(-FLAGS.y_var,FLAGS.y_var)
  z += np.random.uniform(-FLAGS.z_var,FLAGS.z_var)
  yaw += np.random.uniform(-FLAGS.yaw_var,FLAGS.yaw_var)

  return x, y, z, yaw 

# ensure location for logging the xterm outputs exists.
gazebo_xterm_log_dir="{0}{1}/xterm_gazebo".format(FLAGS.summary_dir,FLAGS.log_tag)
if not os.path.isdir(gazebo_xterm_log_dir): os.makedirs(gazebo_xterm_log_dir)

# Some local variables for running different simulations
prev_environment_arguments=''
reset_gazebo_service=rospy.ServiceProxy('/gazebo/reset_simulation',Emptyservice)
model_state_gazebo_service=rospy.ServiceProxy('/gazebo/set_model_state',SetModelState)
unpause_physics_client=rospy.ServiceProxy('/gazebo/unpause_physics',Emptyservice)

gazebo_popen=None
prev_stat_nn_log=''
prev_stat_fsm_log=''
fsm_file = FLAGS.log_folder+'/fsm_log'
if not os.path.isfile(fsm_file):
  with open(fsm_file,'a') as f: 
    f.write('{0}: {1}\n'.format(time.strftime("%Y-%m-%d_%I-%M-%S"), FLAGS.log_folder))


crashed=False 
while (run_number < FLAGS.number_of_runs) or FLAGS.number_of_runs==-1:
  
  ######################################
  # 4.1 Prepare Run
  world_name = FLAGS.worlds[run_number%len(FLAGS.worlds)]

  # save current status of NN nn_ready to compare afterwards
  if os.path.isfile(FLAGS.log_folder+'/nn_ready'):
    prev_stat_nn_log=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
  else: # we have last communication with our log folder so exit with code 2
    myprint("{2}: lost communication with our log folder {0} on host {1} so exit with code 3.".format(FLAGS.log_folder, FLAGS.condor_host, time.strftime("%Y-%m-%d_%I:%M:%S")))
    kill_combo()
    sys.exit(3)

  # clean up gazebo ros folder every now and then
  if run_number%50 == 0 : shutil.rmtree("{0}/.gazebo/log".format(os.environ['HOME']),ignore_errors=True)
  
  evaluate=((run_number%FLAGS.evaluate_every) == 0 and run_number != 0 and FLAGS.evaluate_every != -1) or FLAGS.evaluation
  
  # if evaluate:
  #   rospy.set_param('max_duration', 120)
  # else:
  #   rospy.set_param('max_duration', 5)

  new_environment_arguments=create_environment(run_number, world_name)
  
  ######################################
  # 4.2 Create environment and perform next run

  if rospy.has_param('/starting_positions'):
    starting_positions = rospy.get_param('starting_positions')
    if isinstance(starting_positions,str):
      starting_positions=ast.literal_eval(starting_positions)
  else:
    starting_positions = []

  if (new_environment_arguments == prev_environment_arguments or len(new_environment_arguments) == 0) and not crashed and gazebo_popen != None:
    # 4.2.1 Reset environment for next run if possible
    # 4.2.1a Ensure correct settings
    rospy.set_param('/evaluate',evaluate)
    
    # 4.2.1b Reset environment ==> causes gt_node to freeze for more than a minute...
    # reset_gazebo_service(EmptyRequest())
    
    # 4.2.1c Change position of drone according to new selected starting position
    pose=Pose()
    pose.position.x, pose.position.y, starting_height, yaw = sample_new_position(starting_positions)
    # pose.position.x, pose.position.y, starting_height, yaw=0,0,1,0
    
    myprint("[run_script]: x: {0}, y: {1}, z: {2}, yaw:{3}".format(pose.position.x, pose.position.y, starting_height, yaw))
    # some yaw to quaternion re-orientation code:
    pose.orientation.z=np.sin(yaw)
    pose.orientation.w=np.cos(yaw)
    pose.position.z = 0.1
    model_state = ModelState()
    model_state.model_name = 'quadrotor' if FLAGS.robot.startswith('drone') else 'turtlebot3_burger'
    model_state.pose=pose
    state_request = SetModelStateRequest()
    state_request.model_state = model_state
    retvals = model_state_gazebo_service(state_request)
    rospy.set_param('starting_height', starting_height)
    
    myprint("Changed pose with return values: {0}".format(retvals))
    time.sleep(5) # HAS to be 5 otherwise '/overtake' and '/ready' overlap resulting in empty images in gt_listener
    unpause_physics_client(EmptyRequest())

  else:
    # 4.2.2 Launch Gazebo again
    # 4.2.2a Ensure previous Gazebo is not running anymore
    if gazebo_popen!=None: 
      kill_popen('gazebo', gazebo_popen)
      wait_for_gazebo()

    prev_environment_arguments = new_environment_arguments
    
    # 4.2.2b Build command with correct settings
    # remove if saving location already exists (probably due to crash previously)
    if FLAGS.create_dataset:
      data_location="{0}/{1:05d}_{2}".format(FLAGS.data_location,run_number,world_name)
      if os.path.isdir(data_location): shutil.rmtree(data_location)
      os.makedirs(data_location)
      new_environment_arguments+=" save_images:=true"
      new_environment_arguments+=" data_location:={0}".format(data_location)
      if 'world_file' in new_environment_arguments:
        world_file=[a for a in new_environment_arguments.split(' ') if 'world_file' in a][0].split(':=')[1]
        myprint("[runscript] world_file {0}".format(world_file))
        shutil.copyfile(world_file, data_location+'/'+os.path.basename(world_file))

    x,y,z,yaw=sample_new_position(starting_positions)
    # x,y,z,yaw=-54, -4, 1, -3.14

    command="roslaunch simulation_supervised_demo {0}.launch fsm_config:={1} log_folder:={2} evaluate:={3} {4} graphics:={5} x:={6} y:={7} Yspawned:={9} starting_height:={8} {10}".format(FLAGS.robot,
            FLAGS.fsm,
            FLAGS.log_folder,
            'true' if evaluate else 'false',
            new_environment_arguments,
            'true' if FLAGS.graphics else 'false',
            x,y,z,yaw,
            'random_seed:='+str(FLAGS.random_seed) if FLAGS.random_seed else '')
    # 4.2.2c Launch command
    # Execute command
    myprint( "gazebo_command: {0}".format(command))
    xterm_log_file='{0}/xterm_gazebo_{1}.txt'.format(gazebo_xterm_log_dir,time.strftime("%Y-%m-%d_%I-%M-%S"))
    args = shlex.split("xterm -iconic -l -lf {0} -hold -e {1}".format(xterm_log_file,command))
    gazebo_popen = subprocess.Popen(args)
    pid_gazebo = gazebo_popen.pid


  ######################################
  # 4.3 Wait for run to finish
  # on this moment the run is not crashed (yet).
  crashed=False 
  crash_checked=False
  
  #print starting positions for visualizing later.
  with open(FLAGS.log_folder+'/starting_positions.txt','a') as f:
    f.write('{0}, {1}, {2}\n'.format(x,y,yaw))

  prev_stat_fsm_log=subprocess.check_output(shlex.split("stat -c %Y "+fsm_file))
  time.sleep(0.1)
  myprint("\n{0}: started run {1} of the {2} in {4} {3} {5}".format(time.strftime("%Y-%m-%d_%I:%M:%S"),
                                                          run_number+1, 
                                                          FLAGS.number_of_runs,
                                                          world_name,
                                                          bcolors.OKBLUE,
                                                          bcolors.ENDC))

  start_time=time.time()
  time_spend=0
  # while fsm_file has not been updated, wait...
  while prev_stat_fsm_log == subprocess.check_output(shlex.split("stat -c %Y "+fsm_file)):
    # Check on job suspension: 
    # if between last update and now has been more than 30 seconds (should be less than 0.1s)
    if time.time() - start_time - time_spend > 30:
      myprint("{0}: Job got suspended.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
      time.sleep(30) #wait for big tick to update
      start_time=time.time()
    else:
      time_spend=time.time() - start_time

    # automatically start with /go after 10s
    if FLAGS.auto_go:
      if 10.05 <= time_spend<10.15: 
        go_popen=subprocess.Popen(shlex.split("rostopic pub /go std_msgs/Empty"))
      elif 11.15 <= time_spend < 11.25 and go_popen.poll()==None:
        kill_popen('go', go_popen)
    
    # if False:
    # if time_spend > 60*10 and FLAGS.number_of_runs != 1: #don't interupt if this is a single run
    if time_spend > 5 and not crash_checked:
      crash_checked = True
      # check for crash
      with open(xterm_log_file, 'r') as f:
        for l in f.readlines():
          if 'process has died' in l:
            myprint("[run_script] {0}: found gz crash in {1}: {2}.".format(time.strftime("%Y-%m-%d_%I:%M:%S"), os.path.basename(xterm_log_file),l[:50]))
            crashed=True
            crash_number+=1
      if crashed:
        if crash_number < 10: #after 20 crashes its maybe time to restart everything
          kill_popen('gazebo', gazebo_popen)
        else:
          myprint("{0}: crashed for 10the time so restart everything.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
          kill_combo()
          start_ros()
          start_python()
          crash_number = 0
        break # get out of this loop
    time.sleep(0.1)

  ######################################
  # 4.4 Clean up run
  # 4.4.1 Wait for NN framework if it is running
  if not crashed and 'nn' in FLAGS.fsm:
    # wait for nn_ready and stop in case of no tensorflow communication
    if os.path.isfile(FLAGS.log_folder+'/nn_ready'):
      current_stat=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
      start_time=time.time()
      myprint("{0}: waiting for nn_ready.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))
      while current_stat == prev_stat_nn_log:
        current_stat=subprocess.check_output(shlex.split("stat -c %Y "+FLAGS.log_folder+'/nn_ready'))
        time.sleep(1)
        if time.time()-start_time > 8*60:
          myprint("{0}: waited for 8minutes on nn_ready to finish training so something went wrong on {1} exit with code 2.".format(time.strftime("%Y-%m-%d_%I:%M:%S"), FLAGS.condor_host))
          kill_combo()
          sys.exit(2)
    else:
      myprint("{2}: we have lost communication with our log folder {0} on host {1} so exit with code 3.".format(FLAGS.log_folder, FLAGS.condor_host, time.strftime("%Y-%m-%d_%I:%M:%S")))
      kill_combo()
      sys.exit(3)
  if not crashed:
    message = open(fsm_file,'r').readlines()[-1].strip()
    myprint("{0}: ended run {1} with {3}{2}{4}".format(time.strftime("%Y-%m-%d_%I:%M:%S"), run_number+1, message, bcolors.OKGREEN if 'success' in message else bcolors.FAIL, bcolors.ENDC))
    # increment also in case of crash as drone has zero turning speed:
    run_number+=1
    if message == 'FINISHED': # make this the final run for evaluation
      FLAGS.number_of_runs=run_number+FLAGS.final_evaluation_runs
      FLAGS.evaluation=True

      # run_number = FLAGS.number_of_runs-1
  time.sleep(3) 
  # extra second needed to save image in gt_listener

# after all required runs are finished
kill_combo()
myprint("\n{0}: done.".format(time.strftime("%Y-%m-%d_%I:%M:%S")))


#!/usr/bin/python Run_long_script governs the running of long gazebo_ros_tensorflow simulations.  The core functionality lies in:    1. parsing the correct arguments at different levels (tensorflow dnn, gazebo environment, ros supervision)   2. different crash handling when for instance starting gazebo / tensorflow fails  The script is organized in different steps: 1. Parsing arguments saved in a name space 2. launching ROS  and robot related parameters 3. launching tensorflow in machine (docker/singularity/virtualenv) environment 4. launching experiment with potentially autogenerated gazebo world  Exit code: 0) normal exit code 2) tensorflow stopped working 3) communication with logfolder (Opal) is blocked 4) config file is missing  Example usage: Let behavior arbitration fly with drone through default canyon in singularity environment 1 time while saving images.  python run_script.py --robot drone_sim --fsm oracle_drone_fsm --world canyon --reuse_default_world -n 1 -ds -p params.yaml  Author: <NAME>  Dependecies: simulation_supervised, pilot, klaas_robots # for pausing and unpausing physics engine # to parse startingpositions as string to list Colors to print in terminal with color! # global variables for Popen objects used for terminating sessions Output is not captured on computing cluster,   therefore write it away to logfolder/output as well # Predefined functions. Load yaml as dict and change to proper string arguments.   Note that current implementation will by default change both --key True and --key False to --key. gazebo popen is not enough to get gzserver to stop so wait longer... gazebo popen is not enough to get gzserver to stop so wait longer... Ros might take some time to start the first time so wait till its well in the ps -ef Check status, terminate popen and wait for it to stop. kill ros, python and gazebo pids and wait for them to finish ########################################################################################################################## # STEP 1 Load Parameters Run_simulation_scripts governs the running of long gazebo_ros_tensorflow simulations.         The core functionality lies in:         1. parsing the correct arguments at different levels (tensorflow dnn, gazebo environment, ros supervision)         2. different crash handling when for instance starting gazebo / tensorflow fails # ========================== #   General Settings # ========================== # ========================== #   Robot Settings # ========================== # ========================== #   Tensorflow Settings # ========================== # ========================== #   Environment Settings # ========================== # parser.add_argument("-p","--paramfile",default='eva_params.yaml',type=str, help="Add more parameters to the command loading the DNN in tensorflow ex: eva_params.yaml or params.yaml.") # parser.add_argument("--yaw_var",default=2*3.14,type=float, help="Specify variation in yaw orientation.") # FLAGS=parser.parse_args() # get simulation_supervised dir # 3 main directories have to be defined in order to make it also runnable from a read-only system-installed singularity image. # 1. Tensorflow log directory for saving tensorflow logs and xterm logs # 2. Pilot_data directory for saving data # 3. location for tensorflow code (and also catkin workspace though they are found with rospack) #no explicit directory for code is set so try to parse first from environment # in case environment variable is not set, take home dir # add default values to be able to operate #worlds are appended in a nested list... so get them out. # FLAGS.params=load_param_file(FLAGS.paramfile) if FLAGS.paramfile else "" # check if robot configuration exists is there: # try to extract condor host # Clear log folder if desired # Create main log folder if necessary # Load last position to start from if lastposition is file log folder already existed # in case of data_creation, make data_location in ~/pilot_data # check number of items already recorded # in case there is already data recorded, parse the number of runs and continue from there #assuming number occurs at first 5 digits xxxxx_name_of_data # display and save all settings ########################################################################################################################## # STEP 2 Start ROS with ROBOT specific parameters # ensure location for logging the xterm outputs exists. Start ros core with robot parameters loaded ########################################################################################################################## # STEP 3 Start tensorflow Function that initializes python code. # delete default test folder # if logdir already exists probably condor job is just restarted somewhere so use last saved q in case of training # Add parameters # Create command # Execute command # Wait for creation of tensorflow log file to know the python node is running ########################################################################################################################## # STEP 4 Start gazebo environment Call correct python script for generating potentially new environment.   Returns a string with arguments for the launch file to be concatenated to the launch command. # generate world if it is possible and allowed, this also changes the loaded world file location from the default simsup_demo/worlds to log_folder # don't create a new world if one_world is on # reuse default 10 evaluation canyons or corridors Parse a new x,y,z,yaw(quaternion) pose for the robot given the world name and current robot   returns positions: x, y, z and orientation yaw in quaternion (1 ~ +90) # default with arguments # overwrite sampled starting positions if they were manually set # add some variation # ensure location for logging the xterm outputs exists. # Some local variables for running different simulations ###################################### # 4.1 Prepare Run # save current status of NN nn_ready to compare afterwards # we have last communication with our log folder so exit with code 2 # clean up gazebo ros folder every now and then # if evaluate: #   rospy.set_param('max_duration', 120) # else: #   rospy.set_param('max_duration', 5) ###################################### # 4.2 Create environment and perform next run # 4.2.1 Reset environment for next run if possible # 4.2.1a Ensure correct settings # 4.2.1b Reset environment ==> causes gt_node to freeze for more than a minute... # reset_gazebo_service(EmptyRequest()) # 4.2.1c Change position of drone according to new selected starting position # pose.position.x, pose.position.y, starting_height, yaw=0,0,1,0 # some yaw to quaternion re-orientation code: # HAS to be 5 otherwise '/overtake' and '/ready' overlap resulting in empty images in gt_listener # 4.2.2 Launch Gazebo again # 4.2.2a Ensure previous Gazebo is not running anymore # 4.2.2b Build command with correct settings # remove if saving location already exists (probably due to crash previously) # x,y,z,yaw=-54, -4, 1, -3.14 # 4.2.2c Launch command # Execute command ###################################### # 4.3 Wait for run to finish # on this moment the run is not crashed (yet). #print starting positions for visualizing later. # while fsm_file has not been updated, wait... # Check on job suspension: # if between last update and now has been more than 30 seconds (should be less than 0.1s) #wait for big tick to update # automatically start with /go after 10s # if False: # if time_spend > 60*10 and FLAGS.number_of_runs != 1: #don't interupt if this is a single run # check for crash #after 20 crashes its maybe time to restart everything # get out of this loop ###################################### # 4.4 Clean up run # 4.4.1 Wait for NN framework if it is running # wait for nn_ready and stop in case of no tensorflow communication # increment also in case of crash as drone has zero turning speed: # make this the final run for evaluation # run_number = FLAGS.number_of_runs-1 # extra second needed to save image in gt_listener # after all required runs are finished