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()
