Fixed manual_control usage (#79)

* Fixed manual_control usage
* Move render related code to a dedicated class
* Add tests for render
* Add follow_leading_vehicle example from carla_scenario

Author: Morphlng

src/macad_gym/carla/multi_env.py

@@ -39,7 +39,7 @@
 # from macad_gym.core.sensors.utils import get_transform_from_nearest_way_point
 from macad_gym.carla.reward import Reward
 from macad_gym.core.sensors.hud import HUD
-from macad_gym.viz.render import multi_view_render
+from macad_gym.viz.render import Render
 from macad_gym.carla.scenarios import Scenarios
 
 # The following imports require carla to be imported already.
@@ -278,6 +278,21 @@ def __init__(self, configs=None):
         self._scenario_config = configs["scenarios"]
         self._env_config = configs["env"]
         self._actor_configs = configs["actors"]
+
+        # At most one actor can be manual controlled
+        manual_control_count = 0
+        for _, actor_config in self._actor_configs.items():
+            if actor_config["manual_control"]:
+                if "vehicle" not in actor_config["type"]:
+                    raise ValueError("Only vehicles can be manual controlled.")
+
+                manual_control_count += 1
+
+        assert manual_control_count <= 1, (
+            "At most one actor can be manually controlled. "
+            f"Found {manual_control_count} actors with manual_control=True"
+        )
+
         # Camera position is problematic for certain vehicles and even in
         # autopilot they are prone to error
         self.exclude_hard_vehicles = False
@@ -317,6 +332,24 @@ def __init__(self, configs=None):
         pygame.font.init()  # for HUD
         self._hud = HUD(self._render_x_res, self._render_y_res)
 
+        # For manual_control
+        self._control_clock = None
+        self._manual_controller = None
+        self._manual_control_camera_manager = None
+
+        # Render related
+        Render.resize_screen(self._render_x_res, self._render_y_res)
+        
+        self._camera_poses, window_dim = Render.get_surface_poses(
+            [self._x_res, self._y_res], self._actor_configs)
+
+        if manual_control_count == 0:
+            Render.resize_screen(window_dim[0], window_dim[1])
+        else:
+            self._manual_control_render_pose = (0, window_dim[1])
+            Render.resize_screen(
+                max(self._render_x_res, window_dim[0]), self._render_y_res + window_dim[1])
+
         # Actions space
         if self._discrete_actions:
             self.action_space = Dict(
@@ -633,7 +666,8 @@ def _clear_server_state(self):
         if self._server_process:
             if IS_WINDOWS_PLATFORM:
                 subprocess.call(
-                    ["taskkill", "/F", "/T", "/PID", str(self._server_process.pid)]
+                    ["taskkill", "/F", "/T", "/PID",
+                        str(self._server_process.pid)]
                 )
                 live_carla_processes.remove(self._server_process.pid)
             else:
@@ -693,9 +727,7 @@ def reset(self):
 
         return self._obs_dict
 
-    # TODO: Is this function required?
-    # TODO: Thought: Run server in headless mode always. Use pygame win on
-    # client when render=True
+    # ! Deprecated method
     def _on_render(self):
         """Render the pygame window.
 
@@ -704,11 +736,7 @@ def _on_render(self):
         Returns:
             N/A
         """
-        for cam in self._cameras.values():
-            surface = cam._surface
-            if surface is not None:
-                self._display.blit(surface, (0, 0))
-            pygame.display.flip()
+        pass
 
     def _spawn_new_actor(self, actor_id):
         """Spawn an agent as per the blueprint at the given pose
@@ -867,7 +895,8 @@ def _reset(self, clean_world=True):
                 )
                 actor_config = self._actor_configs[actor_id]
 
-                try:  # Try to spawn actor (soft reset) or fail and reinitialize the server before get back here
+                # Try to spawn actor (soft reset) or fail and reinitialize the server before get back here
+                try:
                     self._actors[actor_id] = self._spawn_new_actor(actor_id)
                 except RuntimeError as spawn_err:
                     del self._done_dict[actor_id]
@@ -928,6 +957,21 @@ def _reset(self, clean_world=True):
                 assert camera_manager.sensor.is_listening
                 self._cameras.update({actor_id: camera_manager})
 
+                # Manual Control
+                if actor_config["manual_control"]:
+                    self._control_clock = pygame.time.Clock()
+                    
+                    self._manual_controller = KeyboardControl(
+                        self, actor_config["auto_control"])
+                    self._manual_controller.actor_id = actor_id
+                    
+                    self.world.on_tick(self._hud.on_world_tick)
+                    self._manual_control_camera_manager = CameraManager(
+                        self._actors[actor_id], self._hud)
+                    self._manual_control_camera_manager.set_sensor(
+                        CAMERA_TYPES['rgb'].value - 1, pos=2, notify=False
+                    )
+
                 self._start_coord.update(
                     {
                         actor_id: [
@@ -1117,10 +1161,13 @@ def step(self, action_dict):
                 k for k, v in self._actor_configs.items() if v.get("render", False)
             ]
             if render_required:
-                images = {k: self._decode_obs(k, v) for k, v in obs_dict.items()}
-                multi_view_render(
-                    images, [self._x_res, self._y_res], self._actor_configs
-                )
+                images = {k: self._decode_obs(k, v)
+                          for k, v in obs_dict.items() if self._actor_configs[k]["render"]}
+
+                Render.multi_view_render(images, self._camera_poses)
+                if self._manual_controller is None:
+                    Render.dummy_event_handler()
+                
             return obs_dict, reward_dict, self._done_dict, info_dict
         except Exception:
             print(
@@ -1166,18 +1213,14 @@ def _step(self, actor_id, action):
 
         config = self._actor_configs[actor_id]
         if config["manual_control"]:
-            clock = pygame.time.Clock()
-            # pygame
-            self._display = pygame.display.set_mode(
-                (config["render_x_res"], config["render_y_res"]),
-                pygame.HWSURFACE | pygame.DOUBLEBUF,
-            )
-            logger.debug("pygame started")
-            controller = KeyboardControl(self, config["auto_control"])
-            controller.actor_id = actor_id
-            controller.parse_events(self, clock)
-            # TODO: Is this _on_render() method necessary? why?
-            self._on_render()
+            self._control_clock.tick(60)
+            self._manual_control_camera_manager._hud.tick(self.world, self._actors[actor_id], self._collisions[actor_id], self._control_clock)
+            self._manual_controller.parse_events(self, self._control_clock)
+
+            # TODO: consider move this to Render as well
+            self._manual_control_camera_manager.render(Render.get_screen(), self._manual_control_render_pose)
+            self._manual_control_camera_manager._hud.render(Render.get_screen(), self._manual_control_render_pose)
+            pygame.display.flip()
         elif config["auto_control"]:
             if getattr(self._actors[actor_id], "set_autopilot", 0):
                 self._actors[actor_id].set_autopilot(
@@ -1480,16 +1523,19 @@ def get_next_actions(measurements, is_discrete_actions):
 
 
 if __name__ == "__main__":
-    argparser = argparse.ArgumentParser(description="CARLA Manual Control Client")
-    argparser.add_argument("--scenario", default="3", help="print debug information")
+    argparser = argparse.ArgumentParser(
+        description="CARLA Manual Control Client")
+    argparser.add_argument("--scenario", default="3",
+                           help="print debug information")
     # TODO: Fix the default path to the config.json;Should work after packaging
     argparser.add_argument(
         "--config",
         default="src/macad_gym/carla/config.json",
         help="print debug information",
     )
 
-    argparser.add_argument("--map", default="Town01", help="print debug information")
+    argparser.add_argument("--map", default="Town01",
+                           help="print debug information")
 
     args = argparser.parse_args()
 

src/macad_gym/core/controllers/keyboard_control.py

@@ -36,6 +36,7 @@
 class KeyboardControl(object):
     """This class from carla, manual_control.py
     """
+
     def __init__(self, world, start_in_autopilot):
         self._autopilot_enabled = start_in_autopilot
         self._control = carla.VehicleControl()
@@ -80,26 +81,44 @@ def parse_events(self, world, clock):
                         self._autopilot_enabled = not self._autopilot_enabled
                         self.vehicle.set_autopilot(self._autopilot_enabled)
             if not self._autopilot_enabled:
+                # FIXME: The actor_id is the name of the vehicle, not the id
                 if self.actor_id == 0:
-                    self._parse_keys1(pygame.key.get_pressed(), clock.get_time())
+                    self._parse_keys1(
+                        pygame.key.get_pressed(), clock.get_time())
                 elif self.actor_id == 1:
-                    self._parse_keys2(pygame.key.get_pressed(), clock.get_time())
-                elif self.actor_id == -1:  # use default ones
-                    self._parse_keys(pygame.key.get_pressed(), clock.get_time())
+                    self._parse_keys2(
+                        pygame.key.get_pressed(), clock.get_time())
+                else:  # use default ones
+                    self._parse_keys(pygame.key.get_pressed(),
+                                     clock.get_time())
                 self.vehicle.apply_control(self._control)
 
     def _parse_keys(self, keys, milliseconds):
-        self._control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0
+        if keys[K_UP] or keys[K_w]:
+            self._control.throttle = min(self._control.throttle + 0.1, 1.0)
+        else:
+            self._control.throttle = 0.0
+
+        if keys[K_DOWN] or keys[K_s]:
+            self._control.brake = min(self._control.brake + 0.2, 1.0)
+        else:
+            self._control.brake = 0
+
         steer_increment = 5e-4 * milliseconds
         if keys[K_LEFT] or keys[K_a]:
-            self._steer_cache -= steer_increment
+            if self._steer_cache > 0:
+                self._steer_cache = 0
+            else:
+                self._steer_cache -= steer_increment
         elif keys[K_RIGHT] or keys[K_d]:
-            self._steer_cache += steer_increment
+            if self._steer_cache < 0:
+                self._steer_cache = 0
+            else:
+                self._steer_cache += steer_increment
         else:
             self._steer_cache = 0.0
         self._steer_cache = min(0.7, max(-0.7, self._steer_cache))
         self._control.steer = round(self._steer_cache, 1)
-        self._control.brake = 1.0 if keys[K_DOWN] or keys[K_s] else 0.0
         self._control.hand_brake = keys[K_SPACE]
 
     def _parse_keys1(self, keys, milliseconds):

src/macad_gym/core/sensors/camera_manager.py

@@ -21,6 +21,7 @@
 class CameraManager(object):
     """This class from carla, manual_control.py
     """
+
     def __init__(self, parent_actor, hud):
         self.image = None  # need image to encode obs.
         self.image_list = []  # for save images later.
@@ -34,9 +35,14 @@ def __init__(self, parent_actor, hud):
         self._camera_transforms = [
             carla.Transform(carla.Location(x=1.8, z=1.7)),
             carla.Transform(carla.Location(x=-5.5, z=2.8),
-                            carla.Rotation(pitch=-15))
+                            carla.Rotation(pitch=-15)),
+            carla.Transform(carla.Location(
+                x=-2.0*(0.5 + self._parent.bounding_box.extent.x),
+                y=0.0,
+                z=2.0*(0.5 + self._parent.bounding_box.extent.z)),
+                carla.Rotation(pitch=8.0))
         ]
-        # 0 is dashcam view; 1 is tethered view
+        # 0 is dashcam view; 1 is tethered view; 2 for spring arm view (manual_control)
         self._transform_index = 0
         self._sensors = [
             ['sensor.camera.rgb', carla.ColorConverter.Raw, 'Camera RGB'],
@@ -114,7 +120,8 @@ def set_sensor(self, index, pos=0, notify=True):
             self.sensor = self._parent.get_world().spawn_actor(
                 self._sensors[index][-1],
                 self._camera_transforms[self._transform_index],
-                attach_to=self._parent)
+                attach_to=self._parent,
+                attachment_type=carla.AttachmentType.Rigid if pos != 2 else carla.AttachmentType.SpringArm)
             # We need to pass the lambda a weak reference to self to avoid
             # circular reference.
             weak_self = weakref.ref(self)
@@ -132,9 +139,9 @@ def toggle_recording(self):
         self._hud.notification('Recording %s' %
                                ('On' if self._recording else 'Off'))
 
-    def render(self, display):
+    def render(self, display, render_pose=(0, 0)):
         if self._surface is not None:
-            display.blit(self._surface, (0, 0))
+            display.blit(self._surface, render_pose)
 
     @staticmethod
     def _parse_image(weak_self, image):

src/macad_gym/core/sensors/hud.py

@@ -38,29 +38,30 @@ def on_world_tick(self, timestamp):
         self.frame_number = timestamp.frame_count
         self.simulation_time = timestamp.elapsed_seconds
 
-    def tick(self, world, clock):
+    def tick(self, world, vehicle, collision_sensor, clock):
         if not self._show_info:
             return
-        t = world.vehicle.get_transform()
-        v = world.vehicle.get_velocity()
-        c = world.vehicle.get_vehicle_control()
+
+        t = vehicle.get_transform()
+        v = vehicle.get_velocity()
+        c = vehicle.get_control()
+
         heading = 'N' if abs(t.rotation.yaw) < 89.5 else ''
         heading += 'S' if abs(t.rotation.yaw) > 90.5 else ''
         heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else ''
         heading += 'W' if -0.5 > t.rotation.yaw > -179.5 else ''
-        colhist = world.collision_sensor.get_collision_history()
+        colhist = collision_sensor.get_collision_history()
         collision = [
             colhist[x + self.frame_number - 200] for x in range(0, 200)
         ]
         max_col = max(1.0, max(collision))
         collision = [x / max_col for x in collision]
-        vehicles = world.world.get_actors().filter('vehicle.*')
+        vehicles = world.get_actors().filter('vehicle.*')
         self._info_text = [
             'Server:  % 16d FPS' % self.server_fps,
             'Client:  % 16d FPS' % clock.get_fps(), '',
             'Vehicle: % 20s' %
-            get_actor_display_name(world.vehicle, truncate=20),
-            'Map:     % 20s' % world.world.map_name,
+            get_actor_display_name(vehicle, truncate=20),
             'Simulation time: % 12s' %
             datetime.timedelta(seconds=int(self.simulation_time)), '',
             'Speed:   % 15.0f km/h' %
@@ -81,7 +82,7 @@ def tick(self, world, clock):
             #                               (l.y - t.location.y)**2 +
             #                               (l.z - t.location.z)**2)
             vehicles = [(self.distance(x.get_location(), t), x)
-                        for x in vehicles if x.id != world.vehicle.id]
+                        for x in vehicles if x.id != vehicle.id]
             for d, vehicle in sorted(vehicles):
                 if d > 200.0:
                     break
@@ -104,12 +105,12 @@ def error(self, text):
         logger.info("Notification error disabled: "+text)
         # self._notifications.set_text('Error: %s' % text, (255, 0, 0))
 
-    def render(self, display):
+    def render(self, display, render_pose=(0,0)):
         if self._show_info:
             info_surface = pygame.Surface((220, self.dim[1]))
             info_surface.set_alpha(100)
-            display.blit(info_surface, (0, 0))
-            v_offset = 4
+            display.blit(info_surface, render_pose)
+            v_offset = 4 + render_pose[1]
             bar_h_offset = 100
             bar_width = 106
             for item in self._info_text: