diff --git a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py index fd5ccbdf6..5505727a9 100644 --- a/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py +++ b/src/giskardpy/tree/behaviors/plot_goal_gantt_chart.py @@ -10,6 +10,7 @@ from giskardpy.motion_graph.monitors.monitors import Monitor, EndMotion, CancelMotion from giskardpy.god_map import god_map from giskardpy.motion_graph.tasks.task import TaskState +import giskardpy.tree.behaviors.plot_motion_graph as plot_motion_graph from giskardpy.tree.behaviors.plugin import GiskardBehavior from giskardpy.utils import logging from giskardpy.utils.decorators import record_time @@ -34,10 +35,11 @@ def plot_gantt_chart(self, goals: List[Goal], monitors: List[Monitor], file_name figure_height = 0.7 + num_bars * 0.19 plt.figure(figsize=(god_map.time * 0.25 + 5, figure_height)) + plt.grid(True, axis='x', zorder=-1) - self.plot_history(task_history, tasks, task_plot_filter) - plt.axhline(y=num_tasks-0.5, color='black', linestyle='--') - self.plot_history(monitor_history, monitors, monitor_plot_filter) + self.plot_task_history(task_history, tasks, task_plot_filter) + plt.axhline(y=num_tasks - 0.5, color='black', linestyle='--') + self.plot_monitor_history(monitor_history, monitors, monitor_plot_filter) plt.xlabel('Time [s]') plt.xlim(0, monitor_history[-1][0]) @@ -46,7 +48,6 @@ def plot_gantt_chart(self, goals: List[Goal], monitors: List[Monitor], file_name plt.ylabel('Tasks | Monitors') plt.ylim(-1, num_bars) plt.gca().yaxis.tick_right() - plt.grid() plt.subplots_adjust(left=0.05, right=0.75) plt.tight_layout() @@ -55,15 +56,11 @@ def plot_gantt_chart(self, goals: List[Goal], monitors: List[Monitor], file_name plt.savefig(file_name) logging.loginfo(f'Saved gantt chart to {file_name}.') - def plot_history(self, - history: List[Tuple[float, List[Optional[TaskState]]]], - things, filter: np.ndarray, - bar_height: float = 0.8): - color_map = {TaskState.not_started: 'white', - TaskState.on_hold: 'gray', - TaskState.running: 'green', - TaskState.failed: (0.85, 0.7, 0.7), - TaskState.succeeded: (0.7, 0.85, 0.7)} + def plot_task_history(self, + history: List[Tuple[float, List[Optional[TaskState]]]], + things, filter: np.ndarray, + bar_height: float = 0.8): + color_map = plot_motion_graph.task_state_to_color state: Dict[str, Tuple[float, TaskState]] = {t.name: (0, TaskState.not_started) for t in things} for end_time, history_state in history: for thing_id, status in enumerate(history_state): @@ -73,27 +70,44 @@ def plot_history(self, start_time, last_status = state[thing.name] if status != last_status: plt.barh(thing.name[:50], end_time - start_time, height=bar_height, left=start_time, - color=color_map[last_status]) - if isinstance(thing, (EndMotion, CancelMotion)) and status == TaskState.running: - plt.barh(thing.name[:50], end_time, height=bar_height/2, color='blue') + color=color_map[last_status], zorder=2) state[thing.name] = (end_time, status) + def plot_monitor_history(self, + history: List[Tuple[float, Tuple[np.ndarray, np.ndarray]]], + things, + filter: np.ndarray, + bar_height: float = 0.8): + color_map = plot_motion_graph.monitor_state_to_color + state = {t.name: (0, 0, TaskState.not_started) for t in things} + for end_time, (bool_states, history_states) in history: + for thing_id, status in enumerate(history_states): + bool_status = bool_states[thing_id] + if not filter[thing_id]: + continue + thing = things[thing_id] + start_time, last_bool_status, last_status = state[thing.name] + outer_color, inner_color = color_map[last_status, last_bool_status] + if status != last_status or bool_status != last_bool_status: + plt.barh(thing.name[:50], end_time - start_time, height=bar_height, left=start_time, + color=outer_color, zorder=2) + plt.barh(thing.name[:50], end_time - start_time, height=bar_height / 2, left=start_time, + color=inner_color, zorder=2) + state[thing.name] = (end_time, bool_status, status) + def get_new_history(self) \ - -> Tuple[List[Tuple[float, List[Optional[TaskState]]]], List[Tuple[float, List[Optional[TaskState]]]]]: + -> Tuple[List[Tuple[float, Tuple[np.ndarray, np.ndarray]]], + List[Tuple[float, List[Optional[TaskState]]]]]: # because the monitor state doesn't get updated after the final end motion becomes true god_map.monitor_manager.evaluate_monitors() - monitor_history: List[Tuple[float, List[Optional[TaskState]]]] = [] - for time_id, (time, (state, life_cycle_state)) in enumerate(god_map.monitor_manager.state_history): - life_cycle_state = copy(life_cycle_state) - for monitor_id in range(len(state)): - if not state[monitor_id] and life_cycle_state[monitor_id] == TaskState.running: - life_cycle_state[monitor_id] = TaskState.on_hold - monitor_history.append((time, life_cycle_state)) # add Nones to make sure all bars gets "ended" new_end_time = god_map.time + god_map.qp_controller_config.sample_period - monitor_history.append((new_end_time, [None] * len(monitor_history[0][1]))) - task_history = god_map.motion_goal_manager.state_history + + monitor_history = copy(god_map.monitor_manager.state_history) + monitor_history.append((new_end_time, ([None] * len(monitor_history[0][1][0]), [None] * len(monitor_history[0][1][0])))) + + task_history = copy(god_map.motion_goal_manager.state_history) task_history.append((new_end_time, [None] * len(task_history[0][1]))) return monitor_history, task_history diff --git a/src/giskardpy/tree/branches/clean_up_control_loop.py b/src/giskardpy/tree/branches/clean_up_control_loop.py index d01c72625..342a2eb01 100644 --- a/src/giskardpy/tree/branches/clean_up_control_loop.py +++ b/src/giskardpy/tree/branches/clean_up_control_loop.py @@ -44,7 +44,7 @@ def add_debug_visualize_trajectory(self): self.add_child(DebugMarkerPublisherTrajectory()) def add_plot_gantt_chart(self): - self.insert_child(PlotGanttChart(), 0) + self.insert_child(PlotGanttChart(), 2) @toggle_on('has_reset_world_state') def add_reset_world_state(self):