plot gantt chart uses graph colors

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

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