Improvements (#57)

judicaelclair · web-flow · commit 8ea33f400184 · 2020-12-31T10:55:02.000Z
* Polished OnlineLSTMMapDisplay + debug mode configuration + bug fixes

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Moved to using direct.stdpy.threading

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Implemented key frame speed

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Properly wired up simulator config options

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Fixed rendering bug in WayPoint

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Implemented ImGui line thickness

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Polished GradientListMapDisplay

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Bug fixes

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Further bug fixes

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Added threading module togglability

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;

* Polished threading module

Signed-off-by: [ 大鳄 ] Asew &lt;clair.judicael@gmail.com&gt;
diff --git a/src/algorithms/algorithm.py b/src/algorithms/algorithm.py
@@ -1,4 +1,4 @@
-from threading import Condition
+from utility.threading import Condition
 from typing import Optional, List
 from abc import ABC, abstractmethod
 
diff --git a/src/algorithms/basic_testing.py b/src/algorithms/basic_testing.py
@@ -1,11 +1,12 @@
-from threading import Condition
 from typing import Dict, Any, List, Callable, Optional
 from operator import mul
 from functools import reduce
+import time
 
 import numpy as np
 from structures.heap import Heap
 
+from utility.threading import Condition, cond_var_wait_for
 from algorithms.configuration.entities.agent import Agent
 from algorithms.configuration.entities.trace import Trace
 from algorithms.configuration.entities.obstacle import Obstacle
@@ -40,6 +41,7 @@ def __init__(self, services: Services) -> None:
         self.cv = None
         self.key_frame_skip_count = 0
         self.total_time = 0
+        self.last_frame_time = 0
         self.requires_key_frame = False
         self.reset()
 
@@ -51,6 +53,7 @@ def reset(self) -> None:
         self.cv = None
         self.key_frame_skip_count = 0
         self.total_time = 0
+        self.last_frame_time = 0
         if self._services.settings.simulator_key_frame_speed == 0 or not self._services.settings.simulator_graphics:
             self.key_frame = lambda *args, **kwargs: None
         else:
@@ -86,6 +89,10 @@ def key_frame_internal(self, ignore_key_frame_skip: bool = False, only_render: L
         if root_key_frame:
             root_key_frame.instance.testing.key_frame(ignore_key_frame_skip=ignore_key_frame_skip, only_render=only_render)
 
+        def sleep_pred():
+            return not self.requires_key_frame or self.is_terminated()
+
+        # skip render frame
         if ignore_key_frame_skip or self.key_frame_skip_count >= self._services.settings.simulator_key_frame_skip:
             self.display_info = self._services.algorithm.instance.set_display_info()
 
@@ -96,11 +103,26 @@ def key_frame_internal(self, ignore_key_frame_skip: bool = False, only_render: L
                 with self.cv:
                     self.requires_key_frame = True
                     self.cv.notify()
-                    self.cv.wait_for(lambda: not self.requires_key_frame or self.is_terminated())
+                    cond_var_wait_for(self.cv, sleep_pred)
             self.key_frame_skip_count = 0
         else:
             self.key_frame_skip_count += 1
 
+        # limit key frame speed
+        if self.cv is not None:
+            t = time.time()
+            sleep_dt = self._services.settings.simulator_key_frame_speed - (t - self.last_frame_time)
+            self.last_frame_time = t
+
+            while sleep_dt > 0:
+                with self.cv:
+                    self.requires_key_frame = True
+                    self.cv.notify()
+                    cond_var_wait_for(self.cv, sleep_pred)
+                nt = time.time()
+                sleep_dt = sleep_dt - nt + t
+                t = nt
+
         if self.is_terminated():
             from simulator.models.map_model import AlgorithmTerminated
             raise AlgorithmTerminated()
diff --git a/src/algorithms/lstm/a_star_waypoint.py b/src/algorithms/lstm/a_star_waypoint.py
@@ -137,10 +137,9 @@ def _find_path_internal(self) -> None:
         local_kernel_algo.find_path(agent_pos=self.way_points[-1], goal_pos=self._get_grid().goal.position)
         local_kernel_algo.map.replay_trace(lambda m: self.__replay(m))
         self.__accumulate_space(local_kernel_algo)
-        self.key_frame()
         self.__local_kernel_anim = None
         self.__global_kernel_anim = None
-        self.key_frame()
+        self.key_frame(True)
 
     def __update_map(self, local_kernel_algo, global_kernel_algo):
         if isinstance(self._get_grid(), RosMap):
diff --git a/src/algorithms/lstm/combined_online_LSTM.py b/src/algorithms/lstm/combined_online_LSTM.py
@@ -1,5 +1,5 @@
 import copy
-from threading import Thread
+from utility.threading import Thread
 from typing import List, Tuple, Optional, Set
 
 from algorithms.algorithm import Algorithm
@@ -26,7 +26,7 @@ def __init__(self, services: Services, testing: BasicTesting = None, kernel_name
                  max_it: float = float('inf'), threaded: bool = False):
         super().__init__(services, testing)
 
-        if not self.kernel_names:
+        if not kernel_names:
             self.kernel_names = [
                 "tile_by_tile_training_house_10_model",
                 "tile_by_tile_training_house_100_model",
diff --git a/src/batch_train.sh b/src/batch_train.sh
@@ -4,4 +4,4 @@
 module load anaconda3/personal
 
 pip3 install -r $HOME/projects/PathBench/requirements.txt
-python3 $HOME/projects/PathBench/src/runtrainer.py -n 30 -f
+python3 $HOME/projects/PathBench/src/run_trainer.py -n 30 -f
diff --git a/src/main.py b/src/main.py
@@ -107,7 +107,7 @@ def main() -> bool:
     parser.add_argument("-v", "--visualiser", action='store_true', help="run simulator with graphics")
     parser.add_argument("-g", "--generator", action='store_true', help="run generator")
     parser.add_argument("-t", "--trainer", action='store_true', help="runs model trainer")
-    parser.add_argument("-d", "--debug", choices=['NONE', 'BASIC', 'LOW', 'MEDIUM', 'HIGH'], default='HIGH', help="set the debug level when running, default is high")
+    parser.add_argument("-d", "--debug", choices=['NONE', 'BASIC', 'LOW', 'MEDIUM', 'HIGH'], default='LOW', help="set the debug level when running, default is low")
     parser.add_argument("-V", dest="visualiser_flags", metavar="VISUALISER_FLAG", action='append',
                         help="Visualiser options (overriding Panda3D's default Config.prc - see https://docs.panda3d.org/1.10/python/programming/configuration/configuring-panda3d#configuring-panda3d for options [windowed-fullscreen is an additional custom option])")
 
diff --git a/src/ros/gazebo/ros.py b/src/ros/gazebo/ros.py
@@ -1,6 +1,5 @@
 import os
 import sys
-from threading import Lock, Condition
 
 import numpy as np
 import cv2 as cv
diff --git a/src/ros/ogm_subscriber/ros.py b/src/ros/ogm_subscriber/ros.py
@@ -1,6 +1,5 @@
 import os
 import sys
-from threading import Lock, Condition
 from typing import Optional
 
 import numpy as np
diff --git a/src/run_trainer.py b/src/run_trainer.py
@@ -43,7 +43,7 @@
 
 import argparse
 
-parser = argparse.ArgumentParser(prog="runtrainer.py",
+parser = argparse.ArgumentParser(prog="run_trainer.py",
                                  description="PathBench trainer runner",
                                  formatter_class=argparse.RawTextHelpFormatter)
 parser.add_argument('-n', '--num_maps', type=int, default=100, help="number of maps to generate for the trainer")
diff --git a/src/simulator/models/map_model.py b/src/simulator/models/map_model.py
@@ -1,6 +1,6 @@
-from threading import Thread, Condition
 import time
 
+from utility.threading import Thread, Condition, cond_var_wait_for
 from algorithms.configuration.maps.dense_map import DenseMap
 from algorithms.configuration.maps.sparse_map import SparseMap
 from simulator.models.model import Model
@@ -60,11 +60,11 @@ def reset(self) -> None:
                 self.requires_key_frame = False
                 self.cv.notify_all()
         with self.cv:
-            self.cv.wait_for(lambda: self.last_thread is None or self.requires_key_frame, timeout=0.8)
+            cond_var_wait_for(self.cv, lambda: self.last_thread is None or self.requires_key_frame, timeout=0.8)
             if self.last_thread is not None:
                 self.requires_key_frame = False
                 self.cv.notify_all()
-                self.cv.wait_for(lambda: self.last_thread is None, timeout=0.8)
+                cond_var_wait_for(self.cv, lambda: self.last_thread is None, timeout=0.8)
             self.requires_key_frame = False
             if self.last_thread is None:
                 self._services.ev_manager.post(KeyFrameEvent(refresh=True))
@@ -107,7 +107,7 @@ def tick(self) -> None:
             dt = self.frame_timer.stop()
             if self.requires_key_frame or (dt < MAX_FRAME_DT):
                 with self.cv:
-                    if self.cv.wait_for(lambda: self.requires_key_frame or self.last_thread is None, timeout=(MAX_FRAME_DT - dt)):
+                    if cond_var_wait_for(self.cv, lambda: self.requires_key_frame or self.last_thread is None, timeout=(MAX_FRAME_DT - dt)):
                         self.processing_key_frame = True
                         self._services.ev_manager.post(KeyFrameEvent())
                 self.frame_timer = Timer()
@@ -149,11 +149,11 @@ def toggle_convert_map(self) -> None:
         else:
             self._services.debug.write("Map conversion not applicable", DebugLevel.BASIC)
             return
-        
+
         if mp is None:
             self._services.debug.write("Map conversion not applicable", DebugLevel.BASIC)
             return
-        
+
         self.reset()
         timer: Timer = Timer()
         self._services.algorithm.map = mp
diff --git a/src/simulator/services/algorithm_runner.py b/src/simulator/services/algorithm_runner.py
@@ -1,5 +1,4 @@
 import copy
-from threading import Condition
 from typing import Optional, Type, TYPE_CHECKING, List, Any, Tuple, Dict
 
 from algorithms.configuration.configuration import Configuration
diff --git a/src/simulator/services/graphics/renderer.py b/src/simulator/services/graphics/renderer.py
@@ -10,14 +10,16 @@ class Renderer():
     __roots: List[NodePath]
     __draw_nps: List[NodePath]
     __circles: List[Tuple[int, float, Geom]]
+    __thicknesses: List[float]
     __line_segs: LineSegs
 
     def __init__(self, root: NodePath) -> None:
         self.__roots = [root]
         self.__draw_nps = []
         self.__circles = []
+        self.__thicknesses = [2.5]
         self.__line_segs = LineSegs()
-        self.__line_segs.set_thickness(4)
+        self.__line_segs.set_thickness(2.5)
 
     def push_root(self, np: NodePath) -> None:
         assert isinstance(np, NodePath)
@@ -28,6 +30,16 @@ def pop_root(self) -> NodePath:
         self.render()
         return self.__roots.pop()
 
+    def push_line_thickness(self, thickness: float) -> None:
+        self.__render_lines()
+        self.__thicknesses.append(thickness)
+        self.__line_segs.set_thickness(thickness)
+
+    def pop_line_thickness(self) -> float:
+        self.__render_lines()
+        self.__line_segs.set_thickness(self.__thicknesses[-2])
+        return self.__thicknesses.pop()
+
     @property
     def line_segs(self) -> LineSegs:
         return self.__line_segs
diff --git a/src/simulator/views/gui/simulator_config.py b/src/simulator/views/gui/simulator_config.py
@@ -3,6 +3,8 @@
 from direct.showbase.ShowBase import ShowBase
 from direct.showbase.DirectObject import DirectObject
 
+import numpy as np
+
 from typing import Tuple, Union, List, Any, Dict, Callable
 import traceback
 
@@ -125,9 +127,9 @@ class SimulatorConfig(DirectObject):
 
     __animations = {
         "None": (0, 0),
-        "Normal": (0.00001, 0),
+        "Normal": (np.finfo(float).eps, 0),
         "Slow": (0.5, 0),
-        "Fast": (0.16, 20)
+        "Fast": (np.finfo(float).eps, 20)
     }
 
     def __init__(self, services: Services, mouse1_press_callbacks: List[Callable[[], None]]):
@@ -147,7 +149,7 @@ def __init__(self, services: Services, mouse1_press_callbacks: List[Callable[[],
                 [], {"global_kernel": (CombinedOnlineLSTM, ([], {})), "global_kernel_max_it": 100})),
             "LSTM Bagging": (CombinedOnlineLSTM, CombinedOnlineLSTMTesting, ([], {})),
             "CAE Online LSTM": (
-                OnlineLSTM, BasicTesting, ([], {"load_name": "tile_by_tile_training_uniform_random_fill_3000_block_map_3000_house_3000_model"})),
+                OnlineLSTM, BasicTesting, ([], {"load_name": "caelstm_section_cae_training_house_100_model"})),
             "Online LSTM": (OnlineLSTM, BasicTesting, (
                 [],
                 {"load_name": "tile_by_tile_training_uniform_random_fill_3000_block_map_3000_house_3000_model"})),
diff --git a/src/simulator/views/map/display/gradient_list_map_display.py b/src/simulator/views/map/display/gradient_list_map_display.py
@@ -52,10 +52,16 @@ def __init__(self, services: Services,
 
         self.__cube_colours = {}
 
-    def get_colour(self, val: float) -> Colour:
+    def get_colour(self, val: float) -> Optional[Colour]:
+        mag = (val - self.min_val)
+
         d = (self.max_val - self.min_val)
-        d = d if d != 0 else 1
-        mag = (val - self.min_val) / d
+        if d != 0:
+            mag /= d
+
+        # catch NaN in addition to out-of-bounds
+        if not (mag >= 0 and mag <= 1):
+            return None
 
         cmag = Colour(mag, mag, mag, mag)
         cmin = self.__deduced_min_colour
@@ -123,7 +129,9 @@ def __eager_colour_update_dispatch(self) -> None:
 
     def update_cube(self, p: Point) -> None:
         if p in self.__cube_colours:
-            self._root_view.colour_cube(self.__cube_colours[p])
+            clr = self.__cube_colours[p]
+            if clr is not None:
+                self._root_view.colour_cube(clr)
 
     def get_tracked_data(self) -> List[Tracked]:
         return self.__tracked_data
diff --git a/src/simulator/views/map/display/online_lstm_map_display.py b/src/simulator/views/map/display/online_lstm_map_display.py
@@ -1,5 +1,5 @@
 import copy
-from typing import Union, Tuple
+from typing import Union, Tuple, Optional
 
 import torch
 import numpy as np
@@ -11,41 +11,51 @@
 from algorithms.lstm.map_processing import MapProcessing
 from simulator.services.services import Services
 from simulator.views.map.display.map_display import MapDisplay
-from structures import Point, Colour, RED
+from structures import Point, Colour, DynamicColour, RED
 
 
 class OnlineLSTMMapDisplay(MapDisplay):
-    def __init__(self, services: Services, custom_map: Map = None):
+    ray_colour: DynamicColour
+    bearing_colour: DynamicColour
+
+    def __init__(self, services: Services, ray_colour: Optional[DynamicColour] = None, bearing_colour: Optional[DynamicColour] = None, custom_map: Map = None):
         super().__init__(services, z_index=250, custom_map=custom_map)
 
+        self.ray_colour = self._services.state.add_colour("ray", RED) if ray_colour is None else ray_colour
+        self.bearing_colour = self._services.state.add_colour("bearing", Colour(1, 0, 1)) if bearing_colour is None else bearing_colour
+
     def render(self, *discarded) -> None:
-        mp = copy.deepcopy(self._map)
+        self._services.graphics.renderer.push_line_thickness(8)
 
-        features = MapProcessing.extract_features(mp, [
+        features = MapProcessing.extract_features(self._map, [
             "distance_to_goal_normalized",
             "raycast_8_normalized",
             "direction_to_goal_normalized",
             "agent_goal_angle"]
         )
 
-        for (ray, dir) in zip(features["raycast_8_normalized"], mp.ALL_POINTS_MOVE_VECTOR):
-            ray = 50.0 * ray - mp.get_movement_cost(Point(0, 0), dir)
+        rc = self.ray_colour()
+        for (ray, dir) in zip(features["raycast_8_normalized"], self._map.ALL_POINTS_MOVE_VECTOR):
+            ray = 50.0 * ray - self._map.get_movement_cost(Point(0, 0), dir)
             dir = dir.to_tensor() / torch.norm(dir.to_tensor())
-            vec = Point.from_tensor(mp.agent.position.to_tensor() + ray * dir)
-            self.__render_line(mp.agent.position, vec, RED)
+            vec = Point.from_tensor(self._map.agent.position.to_tensor() + ray * dir)
+            self.__render_line(self._map.agent.position, vec, rc)
+
+        bc = self.bearing_colour()
+        self.__render_line(self._map.agent.position, self._map.goal.position, bc)
+        self.__render_arc(self._map.agent.position, self._map.goal.position, bc)
 
-        self.__render_line(mp.agent.position, mp.goal.position, Colour(1, 0, 1))
-        self.__render_arc(mp.agent.position, mp.goal.position, Colour(1, 0, 1))
+        self._services.graphics.renderer.pop_line_thickness()
 
     def __render_arc(self, p1: Point, p2: Point, colour: Colour):
         center = p1
         radius = 1
-        
+
         dir = p2.to_tensor() - p1.to_tensor()
         angle = torch.atan2(dir[1], dir[0])
         angle %= 2 * np.pi
 
         self.get_renderer_view().draw_arc(center, angle, radius=radius, colour=colour)
-        
+
     def __render_line(self, p1: Point, p2: Point, colour: Colour):
         self.get_renderer_view().draw_line(colour, p1, p2)
diff --git a/src/utility/misc.py b/src/utility/misc.py
@@ -1,7 +1,9 @@
 import numpy as np
+from utility.threading import Condition
 
 from collections.abc import Iterable
-from typing import Tuple
+from typing import Tuple, Callable, Optional
+import time
 
 
 def fmt_row(width, row):
@@ -41,7 +43,7 @@ def print_header():
 def exclude_from_dict(d, keys):
     return {key: d[key] for key in d if key not in keys}
 
-def flatten(l, ignored_values = []):
+def flatten(l, ignored_values=[]):
     for el in l:
         if isinstance(el, Iterable) and not isinstance(el, (str, bytes)):
             for el2 in flatten(el):
diff --git a/src/utility/threading.py b/src/utility/threading.py
diff --git a/tests/test_graphics/common.py b/tests/test_graphics/common.py

Original file line number	Diff line number	Diff line change
`@@ -1,4 +1,4 @@`
`1`		`-from threading import Condition`
	`1`	`+from utility.threading import Condition`
`2`	`2`	`from typing import Optional, List`
`3`	`3`	`from abc import ABC, abstractmethod`
`4`	`4`