def talker(): pub=rospy.Publisher('PlatoonChannel',String,queue_size=10) rospy.init_node('talker1',anonymous=True) rate=rospy.Rate(10) input_gen=Input() while not rospy.is_shutdown(): keyval=input_gen.send(2) keyval=str(keyval) rate.sleep() #Set Platoon ID if(keyval=='a'): message="a,1,0,1,2,3,4,5,6,7,8,9" #rospy.loginfo(message) pub.publish(message) rate.sleep() #Set robots to follower if(keyval=='c'): message="c,1,2,0,1,2,3,4,5,6,7,8,9" pub.publish(message) rate.sleep() #Set Lane id if(keyval=='d'): message="d,1,1,0,1,2,3,4,5,6,7,8,9" pub.publish(message) rate.sleep() #Set platoon leader if(keyval=='e'): message="e,1,4,6" pub.publish(message) rate.sleep() #Set Speed to leader if(keyval=='g'): message="g,1,4,65" pub.publish(message) rate.sleep() #Set stop to leader if(keyval=='h'): message="h,1,4,0" pub.publish(message) rate.sleep() #Set start to leader if(keyval=='i'): message="i,1,4,1" pub.publish(message) rate.sleep() #Set lane change command to all robots left if(keyval=='m'): message="m,1,10,0,1,2,3,4,5,6,7,8,9" pub.publish(message) rate.sleep() #Set lane change command to all robots right if(keyval=='n'): message="n,1,20,0,1,2,3,4,5,6,7,8,9" pub.publish(message) rate.sleep()
class TrainPlan(): SUCCESS, SKIPPED, CAFFE_TERMINATED, EXISTS, RETRY, USER_ASK_EXIT = range(6) SIGINT, NEWLINE = range(2) def __init__(self, file_name): self._train_plan_filename = file_name with open(file_name, 'r') as f: def filter_comments(line): in_string = False string_char = None for i in range(len(line)): if in_string: if line[i] == string_char: in_string = False else: if line[i] == '#': return line[:i] if line[i] in ['"', "'"]: string_char = line[i] in_string = True return line filtered_file = '\n'.join([filter_comments(line) for line in f]) data = json.loads(filtered_file) try: self._user_ask_exit = False self._name = data['name'] self._params = HyperParameters(data['hyper_params'], data['search_type']) try: self._advanced_rendering_engine = data['rendering_engine'] except KeyError: self._advanced_rendering_engine = False if self._advanced_rendering_engine: from jinja2 import Environment self._templateGenerator = Environment() self._templateGenerator.filters['bool'] = lambda x: str( x).lower() try: self._max_data_points = data['max_data_points'] except KeyError: if data['search_type'] == 'random': error( 'Invalid train plan! When using random search you must set max_data_points.' ) self._max_data_points = None try: self._weights_file = data['weights_file'] except KeyError: self._weights_file = None self._mva_window_size = 10 #default size try: if data['moving_avg_window_size'] > 0: self._mva_window_size = int( data['moving_avg_window_size']) else: error( 'moving_avg_window_size must be positive integer. Using default value of ' '%d' % self._mva_window_size) except KeyError: pass # Use the default value self._termination_rules = dict() self._termination_rules['nan'] = True self._termination_rules['delta'] = -float('inf') try: t = data['termination_rules'] for key, value in t.iteritems(): self._termination_rules[key] = value except KeyError: pass self._runs_history = [] except: error('Invalid train plan!\n') def name(self): return self._name def execute(self, template_net, gpu_id=None): directory = os.path.abspath(self.name()) base_name = os.path.normpath(self.name()).replace('/', '_') try: os.makedirs(directory) except OSError: if not os.path.isdir(directory): raise target_train_plan = os.path.join(directory, 'train_plan.json') if not os.path.isfile(target_train_plan): shutil.copyfile(self._train_plan_filename, target_train_plan) elif not filecmp.cmp(self._train_plan_filename, target_train_plan): error( 'Train plan present in target directory is different than the input train plan.' ) template_net_file = os.path.join(directory, 'net.prototmp') if not os.path.isfile(template_net_file): with open(template_net_file, 'wb') as f: f.write(template_net) else: with open(template_net_file, 'rb') as f: other_template = f.read() if other_template != template_net: error( 'The net template in target directory is different than the input template.' ) unique_csv_file = '%s/%s_unique%s.csv' % (directory, base_name, semi_unique()) all_keys = set() update_csv_interval = random.randint(3, 7) iterations_since_update = 0 def extra_bytes(x): pass self._last_reprint = None self._window_context = CursorAwareWindow( hide_cursor=False, keep_last_line=True, extra_bytes_callback=extra_bytes) self._inside_window_context = False self._input_generator = Input(sigint_event=True) computer_name = platform.node() with self._input_generator: for indices, params in itertools.islice(self._params.search_iter(), self._max_data_points): should_break = False try: indices = list(indices) except: indices = [indices] params = list(params) indices_str = '_'.join(map(str, indices)) params.extend( list( itertools.imap(lambda x: ('run_idx_%d' % x[0], x[1]), enumerate(indices, 1)))) while True: params_dict = dict(params) base_filename = '%s/%s_%s' % (directory, base_name, indices_str) params_dict['name'] = base_filename if not params_dict.has_key('caffe_random_seed'): params_dict['caffe_random_seed'] = random.randint( 0, 0xffffffff) if self._advanced_rendering_engine: template = self._templateGenerator.from_string( template_net) net = template.render(params_dict) else: net = template_net % params_dict weights_file = self._weights_file if weights_file is not None: weights_file_glob = self._templateGenerator.from_string( self._weights_file).render(params_dict) weights_files = glob.glob( os.path.normpath( os.path.join(directory, weights_file_glob))) weights_files.sort(key=os.path.getmtime) if len(weights_files) == 0: self._print( 'Cannot find weights file. Skipped run!') break weights_file = weights_files[-1] description = ('i: %s | ' % indices_str) + ' | '.join( sorted( map( lambda x: '{}: {}'.format( x[0], ensure_precision(x[1], 4)), params))) solve_state, run_params = self._solve( params_dict['name'], description, net, weights_file, gpu_id) if solve_state == TrainPlan.EXISTS: break for key, value in params_dict.iteritems(): run_params['hyper__' + key] = value run_params['run_id'] = indices_str if len( indices) > 1 else indices[0] run_params['computer_name'] = computer_name self._runs_history.append(flatten(run_params)) all_keys.update(self._runs_history[-1].keys()) with open(unique_csv_file, 'w') as csvfile: writer = csv.DictWriter(csvfile, fieldnames=all_keys) writer.writeheader() writer.writerows(self._runs_history) iterations_since_update += 1 if iterations_since_update > update_csv_interval: self._update_main_csv(directory, base_name) iterations_since_update = 0 if solve_state == TrainPlan.CAFFE_TERMINATED: ans = '' while ans.lower() not in ['y', 'n']: ans = self._raw_input( 'Previous run has stopped unexpectedly. Continue? [y/n]' ) if ans.lower() == 'n': should_break = True break elif solve_state == TrainPlan.SKIPPED: self._print('Skipped run!') break elif solve_state == TrainPlan.RETRY: self._print('Retry run!') logfile = '{}.log'.format(params_dict['name']) os.remove(logfile) continue elif solve_state == TrainPlan.USER_ASK_EXIT: should_break = True break break if should_break: break self._input_generator = None if self._inside_window_context: self._inside_window_context.__exit__() self._inside_window_context = False self._last_reprint = None self._window_context = None self._update_main_csv(directory, base_name) def _update_main_csv(self, directory, base_name): regex = re.compile('.*_unique[0-9]+\.csv$') all_keys = set() rows = [] for filename in filter(lambda x: regex.match(x) is not None, os.listdir(directory)): try: with open('%s/%s' % (directory, filename), 'rb') as incsv: reader = csv.DictReader(incsv) for row in reader: rows.append(row) all_keys.update(reader.fieldnames) except: pass all_keys = sorted(all_keys) with open('%s/%s.csv' % (directory, base_name), 'wb') as outcsv: writer = csv.DictWriter(outcsv, fieldnames=all_keys) writer.writeheader() writer.writerows(rows) def _string_to_fsarray(self, msg): if not self._inside_window_context: raise RuntimeError( 'Calling _string_to_fsarray outside of window context') rows, columns = self._window_context.get_term_hw() msg = fmtstr(msg) arr = FSArray(0, columns) i = 0 lines = msg.split('\n') if '\n' in str(msg) else [msg] for line in lines: for j in xrange(len(line)): c = line[j] if i >= rows * columns: return arr else: arr[i // arr.width, i % arr.width] = [c] i += 1 i = ((i // columns) + 1) * columns if len(arr) == 0: return fsarray(['']) return arr def _print(self, s, add_new_line=True, bypass_curtsies=True): if self._last_reprint is not None: if self._inside_window_context: self._window_context.__exit__(None, None, None) self._inside_window_context = False self._last_reprint = None new_string = s if add_new_line: new_string += '\n' if bypass_curtsies: sys.stdout.write(str(new_string)) else: with self._window_context: self._inside_window_context = True fsarr = self._string_to_fsarray(new_string) (totwidth, height), width = self._actual_size(fsarr) self._window_context.render_to_terminal( fsarr, cursor_pos=(max(height - 1, 0), max(width - 1, 0))) self._inside_window_context = False def _reprint(self, s, add_new_line=True): new_reprint = s if add_new_line: new_reprint += '\n' if new_reprint == self._last_reprint: return else: self._last_reprint = new_reprint if not self._inside_window_context: self._window_context.__enter__() self._inside_window_context = True fsarr = self._string_to_fsarray(self._last_reprint) (_, height), width = self._actual_size(fsarr) self._window_context.render_to_terminal(fsarr, cursor_pos=(max(height - 1, 0), max(width - 1, 0))) def _actual_size(self, fsarr): if not self._inside_window_context: raise RuntimeError( 'Calling _actual_size outside of window context') width = fsarr.width height = fsarr.height last_width = fsarr[max(height - 1, 0)].width return ((width, height), last_width) def _raw_input(self, s): self._get_all_input() base_str = s self._reprint(base_str, add_new_line=False) ans = '' last = '' while last != '<Ctrl-j>': last = self._input_generator.next() if type(last) != unicode: continue if last == '<BACKSPACE>': ans = ans[:-1] elif last == '<SPACE>': ans += ' ' elif last != '<Ctrl-j>': ans += last self._reprint(base_str + ' ' + ans, add_new_line=False) self._print(' ') self._get_all_input() return ans def _special_user_input(self): inputs = self._get_all_input() res = [] for inp in inputs: if type(inp) == events.SigIntEvent: res.append(TrainPlan.SIGINT) break for inp in inputs: if type(inp) == unicode and inp == '<Ctrl-j>': res.append(TrainPlan.NEWLINE) break return res def _get_all_input(self): inputs = [] while True: res = self._input_generator.send(0) if res is not None: inputs.append(res) else: break return inputs def _solve(self, name, description, net, weights_file, gpu=None): logfile = '{}.log'.format(name) # Check if file already exists and if so don't solve this case os.utime( os.path.dirname(logfile), None) # touch the directory to refresh the filelisting's cache if os.path.isfile(logfile): return (TrainPlan.EXISTS, None) with open(logfile, 'a'): pass self._print('===================') self._print(description) # try: prototxt = name + '.prototxt' with open(prototxt, 'wb') as f: f.write(net) with open(logfile, 'wb') as f: self._loss_history = [] cmd = "%s train -solver=%s" % (LOCAL_CAFFE_EXEC, prototxt) if weights_file is not None: cmd += " -weights=%s" % weights_file if gpu is not None: cmd += " -gpu=%s" % (str(gpu)) handler = signal.getsignal(signal.SIGINT) signal.signal(signal.SIGINT, signal.SIG_IGN) p = subprocess.Popen(cmd, stderr=subprocess.PIPE, stdout=subprocess.PIPE, shell=True) signal.signal(signal.SIGINT, handler) it = iterate_and_write( subprocess_lines_iterator(p, output='stderr'), f) logParser = LogParser(it, self._mva_window_size) params = logParser.parse_and_react( lambda params: self._react_to_log_parser(params, p)) self._print(' ') code = p.poll() if code is None: p.terminate() if not p.poll(): p.kill() if logParser.last_code() != LogParser.STOP: error('Caffe subprocess has not terminated as expected.') if logParser.last_code() == LogParser.STOP: return (logParser.extra_reaction_info(), params) if code is not None and code != 0: return (TrainPlan.CAFFE_TERMINATED, params) return (TrainPlan.SUCCESS, params) # except: # error('Error while calling caffe executable.') def _react_to_log_parser(self, params, process): ret_value = (LogParser.CONTINUE, None) line_data = [] iteration = None if params['iterations']: iteration = int(params['iterations']) line_data.append(('iters', iteration)) if params['last_minibatch_loss']: if self._termination_rules['nan'] and math.isnan( params['last_minibatch_loss']): ret_value = (LogParser.STOP, TrainPlan.SKIPPED) loss = float(params['last_minibatch_loss']) line_data.append(('loss', loss)) if iteration is not None and ( len(self._loss_history) == 0 or self._loss_history[-1][0] < iteration): self._loss_history.append((iteration, loss)) if params['last_lr']: line_data.append(('lr', params['last_lr'])) for i, test_res in enumerate(params['test_results']): def format_percentage(x): try: float_value = float(x) return '%.2f%%' % ( 100 * float_value) if float_value < 1.0 else '100%' except ValueError: return x loss = next(itertools.ifilter(lambda x: x[0] == 'loss', test_res), None) if loss: if self._termination_rules['nan'] and 'nan' in loss[1]: ret_value = (LogParser.STOP, TrainPlan.SKIPPED) accuracy = next( itertools.ifilter(lambda x: x[0] == 'accuracy', test_res), None) if accuracy: formatted_value = format_percentage(accuracy[1]) best_accuracy = next( itertools.ifilter(lambda x: x[0] == 'best_accuracy', test_res), None) moving_avg = next( itertools.ifilter(lambda x: x[0] == 'moving_avg', test_res), None) if moving_avg: avg_formatted_value = format_percentage(moving_avg[1]) else: avg_formatted_value = '' if best_accuracy: best_formatted_value = format_percentage(best_accuracy[1]) line_data.append(('acc %d' % i, '%s (b: %s, a: %s)' % (formatted_value, best_formatted_value, avg_formatted_value))) else: line_data.append(('acc %d' % i, '%s (a: %s)' % (formatted_value, avg_formatted_value))) n = len(self._loss_history) if n > 10: mid = int(round(n / 2.0)) current_average = np.median( [x for _, x in self._loss_history[mid:]]) previous_average = np.median( [x for _, x in self._loss_history[:mid]]) iter_gap = self._loss_history[-1][0] - self._loss_history[-mid - 1][0] if iter_gap > 0: delta = (current_average - previous_average) / (abs(previous_average) * iter_gap) line_data.append(('rel_avg_delta', delta)) if delta > -self._termination_rules['delta']: ret_value = (LogParser.STOP, TrainPlan.SKIPPED) self._reprint( bold( fmtstr(' | ').join([ underline(key) + ':' + (' %s' % (ensure_precision(value, 2), )) for key, value in line_data ]))) user_input = self._special_user_input() if TrainPlan.NEWLINE in user_input: self._print(' ', add_new_line=False) if TrainPlan.SIGINT in user_input: self._print(' ') ans = '' while ans.lower() not in ['s', 'c', 'e', 'r']: ans = self._raw_input( 'Do you want to (s)kip this run, (c)ontinue running, (r)etry or (e)xit? ' ) if ans.lower() == 's': return (LogParser.STOP, TrainPlan.SKIPPED) elif ans.lower() == 'e': process.terminate() self._user_ask_exit = True return (LogParser.STOP, TrainPlan.USER_ASK_EXIT) elif ans.lower() == 'r': return (LogParser.STOP, TrainPlan.RETRY) return ret_value
input_generator = Input(keynames="curtsies", sigint_event=True) else: print( "Initiating the GoPiGo processing logic in non-interactive mode.\n" ) while True: # End loop on <ESC> key print("main(" + str(counter) + ")---> carId='" + carId + "' balls_collected='" + str(balls_collected) + "' ball_color='" + ball_color + "' mode='" + mode + "' sensorRate='" + sensor_rate + "'") counter += 1 if (args.nonInteractive is False): key = input_generator.send(1 / 20) if ((key is not None) and (key == '<ESC>')): break if (mode == "automatic"): myCar.SetCarModeLED(myCar.GREEN) elif (mode == "manual"): myCar.SetCarModeLED(myCar.BLUE) elif (mode == "debug"): myCar.SetCarModeLED(myCar.RED) # process any new commands in the queue if (len(action_queue) > 0): action_sequence_complete = False # Processing older action first action = action_queue.popleft()