def next_in_schedule(self):
"""
Checks whether the next action can be executed now (i.e. the current time is within its constraints). If not, delays execution suitably.
"""
if len(self.execution_queue) > 0:
now = rospy.get_rostime()
next_task = self.execution_queue[0]
# get travel time to next task.
travel_time_to_next_task = self._travel_duration_fn(next_task.start_node_id)
rospy.loginfo('start window: %s' % rostime_to_python(next_task.start_after).time())
rospy.loginfo(' now: %s' % rostime_to_python(now).time())
rospy.loginfo(' travel time: %s' % rosduration_to_python(travel_time_to_next_task))
# the task can be executed if we can travel to the task in time for the
# the start window to open
if now >= (next_task.start_after - travel_time_to_next_task):
rospy.loginfo('start window is open')
self.execute_next_task()
else:
# delay is the difference between now and the time to start navigating
exe_delay = (next_task.start_after - travel_time_to_next_task) - now
rospy.loginfo('need to delay %s.%s for execution' % (exe_delay.secs, exe_delay.nsecs))
self.current_task = None
self.execution_delay_timer = rospy.Timer(exe_delay, self.execution_delay_cb, oneshot=True)
else:
self.current_task = None
def insert_extra_tasks(self, tasks):
rospy.loginfo('Trying to insert an extra %s tasks into the running routine' % len(tasks))
with self._state_lock:
self._log_task_events(tasks, TaskEvent.ROUTINE_ADDED, rospy.get_rostime(), "Task was added to the routine.")
extra_tasks = []
dropped_tasks = []
for task in tasks:
task_start = rostime_to_python(task.start_after, tz = self.current_routine_start.tzinfo).time()
task_end = rostime_to_python(task.end_before, tz = self.current_routine_end.tzinfo).time()
if time_greater_than(task_start, self.current_routine_end):
dropped_tasks.append(task)
elif time_less_than(task_end, self.current_routine_start):
dropped_tasks.append(task)
elif time_less_than(task_end, task_start):
dropped_tasks.append(task)
else:
extra_tasks.append(task)
with self._state_lock:
self._log_task_events(dropped_tasks, TaskEvent.DROPPED, rospy.get_rostime(), "Task does not fit into daily start/end window of routine.")
# if no tasks are being delayed then start up
if not self._delaying:
rospy.loginfo('Sending %s tasks to _create_routine' % len(extra_tasks))
self._delaying = True
self._create_routine(extra_tasks)
else:
rospy.loginfo('Appending %s tasks to _extra_tasks' % len(extra_tasks))
self._extra_tasks += extra_tasks
def do_days(self, idx):
try:
idx = int(idx)
if not self.check_idx(idx):
return
window_start = rostime_to_python(self.routine_pairs[idx][0].time,
self.tz)
window_end = rostime_to_python(self.routine_pairs[idx][1].time,
self.tz)
working_day_count = 0
for daily_start, daily_end in daily_windows_in_range(
self.daily_start, self.daily_end, window_start, window_end,
self.days_off):
working_day_count += 1
all_day_count = 0
for daily_start, daily_end in daily_windows_in_range(
self.daily_start, self.daily_end, window_start,
window_end):
all_day_count += 1
print 'This routine covered {0} working days from a total of {1}'.format(
working_day_count, all_day_count)
except ValueError, e:
print 'provided argument was not an int: %s' % idx
def do_executions(self, line):
try:
tokens = line.split(' ')
idx = tokens[0]
if len(tokens) > 1 and len(tokens[1]) > 0:
action = tokens[1]
print 'only showing action: %s' % action
else:
action = None
idx = int(idx)
if not self.check_idx(idx):
return
print 'executions in routine %s' % idx
window_start = rostime_to_python(self.routine_pairs[idx][0].time,
self.tz)
window_end = rostime_to_python(self.routine_pairs[idx][1].time,
self.tz)
results = task_query.query_tasks(
self.msg_store,
event=range(TaskEvent.TASK_STARTED, TaskEvent.ROUTINE_STARTED),
action=action,
start_date=window_start,
end_date=window_end,
)
task_query.executions(results)
except ValueError, e:
print 'provided argument was not an int: %s' % idx
print e
def draw_task(self, y, action, start_time, end_time):
# start and end times are ros times, we need to turn them into just hours
start_midnight = datetime.fromordinal(
rostime_to_python(start_time, self.tz).date().toordinal())
start_midnight = start_midnight.replace(tzinfo=self.tz)
start = (rostime_to_python(start_time, self.tz) -
start_midnight).total_seconds()
end = (rostime_to_python(end_time, self.tz) -
start_midnight).total_seconds()
label = None
if action not in self.colour_mappings:
colour_map = plt.get_cmap('Paired')
self.colour_mappings[action] = colour_map(
len(self.colour_mappings) * 30)
label = action
plt.hlines(y,
start,
end,
self.colour_mappings[action],
lw=6,
label=label)
def gather_paths(start, end):
nav_store = MessageStoreProxy(collection="nav_stats")
task_store = MessageStoreProxy(collection="task_events")
paths = [[]]
for task_group in task_groups_in_window(start,
end,
task_store,
event=[
TaskEvent.NAVIGATION_STARTED,
TaskEvent.NAVIGATION_SUCCEEDED
]):
meta_query = {
"inserted_at": {
"$gte": rostime_to_python(task_group[0].time),
"$lte": rostime_to_python(task_group[-1].time)
}
}
nav_stats = nav_store.query(NavStatistics._type, meta_query=meta_query)
nav_stats.sort(key=lambda x: x[1]["epoch"])
for (stat, meta) in nav_stats:
# if stat.status == 'success':
# check they're joined up in space and time
if stat.origin != 'none' and stat.final_node != 'none':
if len(paths[-1]) > 0 and paths[-1][-1][
0].final_node == stat.origin and close_in_time(
paths[-1][-1], (stat, meta)):
paths[-1].append((stat, meta))
# if not create a new path
else:
paths.append([(stat, meta)])
else:
paths.append([])
paths.append([])
min_path_length = 2
paths = [path for path in paths if len(path) >= min_path_length]
print len(paths)
example_path = paths[-1]
path_stats = []
for path in paths:
path_stats.append(
(path[0][0].origin, path[-1][0].final_node, path[0][1]['epoch'],
sum([stat.operation_time for (stat, meta) in path])))
path_lengths = [len(path) for path in paths]
print '{0} paths, max len {1}'.format(len(path_stats), max(path_lengths))
# n, bins, patches = plt.hist(path_lengths, bins = max(path_lengths))
# plt.show()
return group_paths(path_stats)
def do_print(self, line):
for i in range(len(self.routine_pairs)):
start = rostime_to_python(self.routine_pairs[i][0].time, self.tz)
end = rostime_to_python(self.routine_pairs[i][1].time, self.tz)
results = task_query.query_tasks(self.msg_store,
start_date=start,
end_date=end,
event=[TaskEvent.TASK_STARTED])
print 'routine %s: %s to %s, duration: %s, tasks: %s' % (
i, start, end, end - start, len(results))
def do_lateness(self, idx):
""" Not recommended for use...
"""
try:
idx = int(idx)
if not self.check_idx(idx):
return
window_start = rostime_to_python(self.routine_pairs[idx][0].time,
self.tz)
window_end = rostime_to_python(self.routine_pairs[idx][1].time,
self.tz)
means = []
stddev = []
count = []
for daily_start, daily_end in daily_windows_in_range(
self.daily_start, self.daily_end, window_start,
window_end):
day_errors = np.array([
((task_group[2].task.execution_time -
task_group[2].time).to_sec() /
(task_group[2].time - task_group[1].time).to_sec())
for task_group in task_groups_in_window(
daily_start,
daily_end,
self.msg_store,
event=[
TaskEvent.ADDED, TaskEvent.NAVIGATION_STARTED,
TaskEvent.NAVIGATION_SUCCEEDED
])
])
if len(day_errors) > 5:
means.append(day_errors.mean())
stddev.append(day_errors.std())
count.append(len(means))
plt.errorbar(count, means, stddev)
plt.show()
except ValueError, e:
print 'provided argument was not an int: %s' % idx
def do_autonomy(self, idx):
try:
idx = int(idx)
if not self.check_idx(idx):
return
window_start = rostime_to_python(self.routine_pairs[idx][0].time,
self.tz)
window_end = rostime_to_python(self.routine_pairs[idx][1].time,
self.tz)
total_autonomy_duration = timedelta(seconds=0)
total_life_duration = timedelta(seconds=1)
for daily_start, daily_end in daily_windows_in_range(
self.daily_start, self.daily_end, window_start, window_end,
self.days_off):
autonomy_duration, day_duration = self.autonomy_day(
daily_start, daily_end)
total_autonomy_duration += autonomy_duration
total_life_duration += day_duration
daily_start = datetime.combine(
(daily_start + timedelta(days=1)).date(), self.daily_start)
daily_end = datetime.combine(
(daily_end + timedelta(days=1)).date(), self.daily_end)
total_autonomy_percentage = (
total_autonomy_duration.total_seconds() /
total_life_duration.total_seconds()) * 100.0
tl_h, tl_m, tl_s = self.to_hours_minutes_seconds(
total_life_duration.total_seconds())
ta_h, ta_m, ta_s = self.to_hours_minutes_seconds(
total_autonomy_duration.total_seconds())
print 'Total life time: %s hours, %s minutes' % (tl_h, tl_m)
print 'Total autonomy: %s hours, %s minutes' % (ta_h, ta_m)
print 'A: %s' % total_autonomy_percentage
except ValueError, e:
print 'provided argument was not an int: %s' % idx
def do_timeplot(self, line):
try:
tokens = line.split(' ')
idx = tokens[0]
filename = tokens[1]
idx = int(idx)
if not self.check_idx(idx):
return
window_start = rostime_to_python(self.routine_pairs[idx][0].time,
self.tz)
window_end = rostime_to_python(self.routine_pairs[idx][1].time,
self.tz)
# get all the task starts
results = task_query.query_tasks(
self.msg_store,
event=TaskEvent.TASK_STARTED,
start_date=window_start,
end_date=window_end,
)
# convert to an array of times
dates = [
rostime_to_python(event[0].time, self.tz) for event in results
]
with PdfPages('{0}_time_plot.pdf'.format(filename)) as pdf:
n, bins, patches = plt.hist(
[date.hour + date.minute / 60.0 for date in dates],
bins=24 * 60 / 15)
# plt.show()
pdf.savefig()
plt.close()
except ValueError, e:
print 'provided argument was not an int: %s' % idx
def do_summarise(self, idx):
try:
idx = int(idx)
if not self.check_idx(idx):
return
window_start = rostime_to_python(self.routine_pairs[idx][0].time,
self.tz)
window_end = rostime_to_python(self.routine_pairs[idx][1].time,
self.tz)
results = task_query.query_tasks(
self.msg_store,
event=range(TaskEvent.TASK_STARTED, TaskEvent.ROUTINE_STARTED),
start_date=window_start,
end_date=window_end,
)
task_query.aggregate(results)
except ValueError, e:
print 'provided argument was not an int: %s' % idx
def print_status(line_limit=20):
if all_tasks_data is None or active_tasks_data is None:
return
active_ids = set(t.task_id for t in active_tasks_data.execution_queue)
non_active_tasks = [
t for t in all_tasks_data.execution_queue
if not t.task_id in active_ids
]
active_tasks = active_tasks_data.execution_queue
line_count = 0
rospy.loginfo("")
rospy.loginfo("")
rospy.loginfo(
datetime.fromtimestamp(rospy.get_rostime().secs).replace(
tzinfo=localtz).strftime("%H:%M:%S %d/%m/%y"))
line_count += 1
if len(active_tasks) > 0:
rospy.loginfo("Current tasks:")
line_count += 1
for t in active_tasks:
rospy.loginfo(" %s" % pretty(t))
line_count += 1
if goal_string is not None:
rospy.loginfo("Policy:")
rospy.loginfo(' as LTL string %s' % goal_string)
line_count += 2
if feedback_data is not None:
rospy.loginfo(' executed action: %s, status %s' %
(feedback_data.feedback.executed_action,
GoalStatus.to_string(
feedback_data.feedback.execution_status)))
rospy.loginfo(' next action: %s' %
(feedback_data.feedback.next_action))
rospy.loginfo(
' policy completing with probability %d, expected time %ds' %
(feedback_data.feedback.probability,
feedback_data.feedback.expected_time.to_sec()))
line_count += 3
rospy.loginfo("Batch started at %s", start_time(active_tasks[0]))
line_count += 1
rospy.loginfo(" finish by %s", end_time(active_tasks[0]))
line_count += 1
else:
rospy.loginfo("No active tasks")
line_count += 1
if len(non_active_tasks) > 0:
rospy.loginfo("Future tasks (in some order):")
line_count += 1
printed = 0
for t in non_active_tasks:
if line_count > line_limit - 2:
rospy.loginfo(" ... and another %s tasks not printed" %
(len(non_active_tasks) - printed))
return
else:
rospy.loginfo(
" %s after %s" %
(pretty(t), rostime_to_python(
t.start_after,
tz=localtz).strftime("%H:%M:%S %d/%m/%y")))
line_count += 1
printed += 1
else:
rospy.loginfo("No additional tasks")
def create_task_summary_docs(event_msg_store,
event_collection,
summary_collection,
reprocess=False):
try:
event_task_ids = set(event_collection.find().distinct('task.task_id'))
summary_task_ids = set(summary_collection.find().distinct('task_id'))
missing_summaries = event_task_ids - summary_task_ids
print '%s tasks missing summaries' % len(missing_summaries)
if reprocess:
missing_summaries = event_task_ids
print 'reprocessing all %s tasks ' % len(missing_summaries)
dead_task_duration = rospy.Duration(60 * 60 * 5)
now = rospy.get_rostime()
for task_id in missing_summaries:
task_events = event_msg_store.query(
TaskEvent._type, message_query={'task.task_id': task_id})
task_events.sort(key=lambda x: x[0].time.to_sec())
first_event = task_events[0][0]
last_event = task_events[-1][0]
# check whether this event is done
last_event_type = last_event.event
if last_event_type == TaskEvent.TASK_SUCCEEDED or \
last_event_type == TaskEvent.TASK_FAILED or \
last_event_type == TaskEvent.DROPPED:
pass
# else if could still be live
elif last_event_type != TaskEvent.ROUTINE_STARTED and last_event_type != TaskEvent.ROUTINE_STOPPED:
last_event_time = last_event.time
duration_since_event = now - last_event_time
if duration_since_event > dead_task_duration:
print 'calling time on %s events for task %s with final event type %s in the last %s' % (
len(task_events), task_id,
task_event_string(last_event_type),
ros_duration_to_string(duration_since_event))
else:
print 'ignoring %s events for task %s with final event type %s in the last %s' % (
len(task_events), task_id,
task_event_string(last_event_type),
ros_duration_to_string(duration_since_event))
continue
summary_doc = {'task_id': task_id}
event_sequence = [
task_event.event for task_event, meta in task_events
]
events = set(event_sequence)
summary_doc['started'] = TaskEvent.TASK_STARTED in events
summary_doc['stopped'] = TaskEvent.TASK_STOPPED in events
summary_doc['dropped'] = TaskEvent.DROPPED in events
summary_doc['preempted'] = TaskEvent.TASK_PREEMPTED in events
summary_doc['succeeded'] = TaskEvent.TASK_SUCCEEDED in events
summary_doc['failed'] = TaskEvent.TASK_FAILED in events
if TaskEvent.DEMANDED in events:
task_type = 'on-demand'
elif last_event.task.start_after == last_event.task.end_before:
task_type = 'time-critical'
else:
task_type = 'normal'
summary_doc['task_type'] = task_type
summary_doc['priority'] = last_event.task.priority
action = last_event.task.action
if len(action) == 0:
action = 'navigation'
elif action[0] == '/':
action = action[1:]
summary_doc['action'] = action
summary_doc['ltl_task'] = ' ' in last_event.task.action
summary_doc['first_event_time'] = rostime_to_python(
first_event.time)
summary_doc['last_event_time'] = rostime_to_python(last_event.time)
summary_doc['start_count'] = event_sequence.count(
TaskEvent.TASK_STARTED)
# print summary_doc
summary_collection.update_one({'task.task_id': task_id},
{'$set': summary_doc},
upsert=True)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
args = parser.parse_args()
try:
start = args.start
end = args.end
results = task_query.query_tasks(
msg_store,
event=[TaskEvent.ROUTINE_STARTED, TaskEvent.ROUTINE_STOPPED],
start_date=start,
end_date=end,
)
for event in results:
task_event = event[0]
event_type = task_event.event
if event_type == TaskEvent.ROUTINE_STARTED:
last_start = rostime_to_python(task_event.time)
append = ''
else:
end_time = rostime_to_python(task_event.time)
append = ', so the robot ran for %s\n' % (end_time -
last_start)
print('%s %s%s' % (task_query.task_event_string(event_type),
task_query.event_time(task_event), append))
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def do_taskplot(self, line):
try:
tokens = line.split(' ')
idx = tokens[0]
filename = tokens[1]
idx = int(idx)
if not self.check_idx(idx):
return
window_start = rostime_to_python(self.routine_pairs[idx][0].time,
self.tz)
window_end = rostime_to_python(self.routine_pairs[idx][1].time,
self.tz)
daily_tasks = []
for daily_start, daily_end in daily_windows_in_range(
self.daily_start, self.daily_end, window_start, window_end,
self.days_off):
succeeded_tasks = np.array([(
task_group[0].task.action, task_group[0].time,
task_group[1].time
) for task_group in task_groups_in_window(
daily_start,
daily_end,
self.msg_store,
event=[TaskEvent.TASK_STARTED, TaskEvent.TASK_SUCCEEDED])],
dtype=object)
failed_tasks = np.array([
(task_group[0].task.action, task_group[0].time,
task_group[1].time)
for task_group in task_groups_in_window(
daily_start,
daily_end,
self.msg_store,
event=[TaskEvent.TASK_STARTED, TaskEvent.TASK_FAILED])
],
dtype=object)
# print len(succeeded_tasks)
# print len(failed_tasks)
# can't concatenate 0 length np.arrays
if len(succeeded_tasks) == 0:
all_tasks = failed_tasks
elif len(failed_tasks) == 0:
# print succeeded_tasks
all_tasks = succeeded_tasks
else:
all_tasks = np.concatenate((succeeded_tasks, failed_tasks),
axis=0)
print daily_start.date(), len(all_tasks)
daily_tasks.append([daily_start.date(), all_tasks])
with PdfPages('{0}_task_plot.pdf'.format(filename)) as pdf:
y_sep = 6
y = 0
mpl.rcParams['font.size'] = 6
#
y_label_points = []
y_labels = []
for task_date, task_times in reversed(daily_tasks):
y += y_sep
y_label_points.append(y)
y_labels.append(task_date.strftime('%A, %B %d %Y'))
for task_time in task_times:
self.draw_task(y, task_time[0], task_time[1],
task_time[2])
plt.ylim(0, y + y_sep)
x_label_points = []
x_labels = []
for hour in range(self.daily_start.hour - 1,
self.daily_end.hour + 1):
seconds_per_hour = 60 * 60
x_label_points.append(hour * seconds_per_hour)
x_labels.append('%s:00' % hour)
plt.xticks(x_label_points, x_labels, rotation='vertical')
plt.yticks(y_label_points, y_labels, rotation='horizontal')
lgd = plt.legend(loc='lower right',
bbox_to_anchor=(1, 1),
ncol=2,
prop={'size': 8})
# plt.gcf().tight_layout()
pdf.savefig(bbox_extra_artists=(lgd, ), bbox_inches='tight')
plt.close()
except ValueError, e:
print 'provided argument was not an int: %s' % idx
