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