Merge branch 'main' of github.com:decisionforce/pgdrive into release-…

…0.1.2

pgdrive/scene_creator/blocks/roundabout.py

@@ -101,7 +101,7 @@ def _create_circular_part(self, road: Road, part_idx: int, radius_exit: float, r
         )
 
         segment_start_node = segment_end_node
-        segment_end_node = self.add_road_node()
+        segment_end_node = self.add_road_node() if part_idx < 3 else self._pre_block_socket.negative_road.start_node
         segment_road = Road(segment_start_node, segment_end_node)
 
         none_cross = CreateRoadFrom(

pgdrive/scene_creator/highway_vehicle/behavior.py

@@ -19,10 +19,10 @@ class IDMVehicle(ControlledVehicle):
     """
 
     # Longitudinal policy parameters
-    ACC_MAX = 6.0  # [m/s2]
+    ACC_MAX = 10.0  # [m/s2]
     """Maximum acceleration."""
 
-    COMFORT_ACC_MAX = 3.0  # [m/s2]
+    COMFORT_ACC_MAX = 5.0  # [m/s2]
     """Desired maximum acceleration."""
 
     COMFORT_ACC_MIN = -5.0  # [m/s2]
@@ -34,7 +34,7 @@ class IDMVehicle(ControlledVehicle):
     TIME_WANTED = 1.5  # [s]
     """Desired time gap to the front vehicle."""
 
-    DELTA = 4.0  # []
+    DELTA = 2.0  # []
     """Exponent of the velocity term."""
 
     # Lateral policy parameters

pgdrive/scene_creator/pg_traffic_vehicle/traffic_vehicle.py

@@ -28,7 +28,9 @@ def reset(self, kinematics_model):
 
 class PgTrafficVehicle(DynamicElement):
     COLLISION_MASK = 4
-    HEIGHT = 2
+    HEIGHT = 1.8
+    LENGTH = 4
+    WIDTH = 2
     path = None
     model_collection = {}  # save memory, load model once
 
@@ -40,9 +42,11 @@ def __init__(self, index: int, kinematic_model: IDMVehicle, enable_reborn: bool
         :param enable_reborn: It will be generated at the born place again when arriving at the destination
         :param np_random: Random Engine
         """
+        kinematic_model.LENGTH = self.LENGTH
+        kinematic_model.WIDTH = self.WIDTH
         super(PgTrafficVehicle, self).__init__()
         self.vehicle_node = PgTrafficVehicleNode(BodyName.Traffic_vehicle, IDMVehicle.create_from(kinematic_model))
-        chassis_shape = BulletBoxShape(Vec3(kinematic_model.LENGTH / 2, kinematic_model.WIDTH / 2, self.HEIGHT / 2))
+        chassis_shape = BulletBoxShape(Vec3(self.LENGTH / 2, self.WIDTH / 2, self.HEIGHT / 2))
         self.index = index
         self.vehicle_node.addShape(chassis_shape, TransformState.makePos(Point3(0, 0, self.HEIGHT / 2 + 0.2)))
         self.vehicle_node.setMass(800.0)
@@ -55,23 +59,16 @@ def __init__(self, index: int, kinematic_model: IDMVehicle, enable_reborn: bool
         self.out_of_road = False
 
         np_random = np_random or get_np_random()
-        [path, scale, zoffset, H] = self.path[np_random.randint(0, len(self.path))]
+        [path, scale, x_y_z_offset, H] = self.path[np_random.randint(0, len(self.path))]
         if self.render:
             if path not in PgTrafficVehicle.model_collection:
                 carNP = self.loader.loadModel(AssetLoader.file_path("models", path))
                 PgTrafficVehicle.model_collection[path] = carNP
             else:
                 carNP = PgTrafficVehicle.model_collection[path]
             carNP.setScale(scale)
-
-            if path == 'new/lada/scene.gltf':
-                carNP.setY(-13.5)
-                carNP.setX(1)
-            if path == 'new/cp/scene.gltf':
-                print('fic')
-
             carNP.setH(H)
-            carNP.setZ(zoffset)
+            carNP.setPos(x_y_z_offset)
 
             carNP.instanceTo(self.node_path)
         self.step(1e-1)

pgdrive/scene_creator/pg_traffic_vehicle/traffic_vehicle_type.py

@@ -3,30 +3,38 @@
 factor = 1
 
 
-class MVehicle(PgTrafficVehicle):
-    LENGTH = 4.2
+class LVehicle(PgTrafficVehicle):
+    LENGTH = 4.8
     WIDTH = 1.8
-    HEIGHT = 1.7
+    HEIGHT = 1.9
     path = [
-        ['new/lada/scene.gltf', (factor * 1.1, factor * 1.1, factor * 1.1), factor * -0.1, 223],
-        ['new/beetle/scene.gltf', (factor * .007, factor * .007, factor * .006), factor * -0.15, -90],
+        ['new/lada/scene.gltf', (factor * 1.1, factor * 1.1, factor * 1.1), (1.1, -13.5, factor * -0.046), 223],
     ]
 
 
 class SVehicle(PgTrafficVehicle):
-    LENGTH = 3.5
+    LENGTH = 3.2
     WIDTH = 1.8
     HEIGHT = 1.5
     path = [
-        ['new/130/scene.gltf', (factor * .0055, factor * .0055, factor * .0055), factor * 0.5, 90],
+        ['new/beetle/scene.gltf', (factor * .008, factor * .006, factor * .0062), (-0.7, 0, factor * -0.16), -90],
     ]
 
 
-class LVehicle(PgTrafficVehicle):
-    LENGTH = 8.0
-    WIDTH = 2.2
-    HEIGHT = 3.5
-    path = [['new/truck/scene.gltf', (factor * 0.025, factor * 0.025, factor * 0.025), factor * 0, 0]]
+class MVehicle(PgTrafficVehicle):
+    LENGTH = 3.9
+    WIDTH = 2.0
+    HEIGHT = 1.3
+    path = [
+        ['new/130/scene.gltf', (factor * .0055, factor * .0046, factor * .0049), (0, 0, factor * 0.33), 90],
+    ]
+
+
+class XLVehicle(PgTrafficVehicle):
+    LENGTH = 7.3
+    WIDTH = 2.3
+    HEIGHT = 2.7
+    path = [['new/truck/scene.gltf', (factor * 0.031, factor * 0.025, factor * 0.025), (0.35, 0, factor * 0), 0]]
 
 
-car_type = {"s": SVehicle, "m": MVehicle, "l": LVehicle}
+car_type = {"s": SVehicle, "m": MVehicle, "l": LVehicle, "xl": XLVehicle}

pgdrive/scene_manager/scene_manager.py

@@ -131,7 +131,7 @@ def _create_vehicles_on_lane(self, lane, is_reborn_lane=False):
             if self.np_random.rand() > self.traffic_density and abs(lane.length - InRampOnStraight.RAMP_LEN) > 0.1:
                 # Do special handling for ramp, and there must be vehicles created there
                 continue
-            vehicle_type = car_type[self.np_random.choice(list(car_type.keys()), p=[0.5, 0.3, 0.2])]
+            vehicle_type = car_type[self.np_random.choice(list(car_type.keys()), p=[0.2, 0.3, 0.3, 0.2])]
             random_v = vehicle_type.create_random_traffic_vehicle(
                 len(self.vehicles), self, lane, long, seed=self.random_seed, enable_reborn=is_reborn_lane
             )

pgdrive/tests/test_pgdrive_env/test_save_episode.py

@@ -0,0 +1,69 @@
+import json
+
+from pgdrive.envs.pgdrive_env import PGDriveEnv
+from pgdrive.scene_creator.map import Map, MapGenerateMethod
+from pgdrive.scene_manager import TrafficMode
+from pgdrive.utils import setup_logger
+
+
+class TestEnv(PGDriveEnv):
+    def __init__(self, save_episode=True, vis=True):
+        super(TestEnv, self).__init__(
+            {
+                "environment_num": 1,
+                "traffic_density": 0.1,
+                "start_seed": 5,
+                "manual_control": vis,
+                "use_render": vis,
+                "traffic_mode": TrafficMode.Reborn,
+                "record_episode": save_episode,
+                "map_config": {
+                    Map.GENERATE_METHOD: MapGenerateMethod.BIG_BLOCK_SEQUENCE,
+                    Map.GENERATE_PARA: "XTXTXTXTXT",
+                    Map.LANE_WIDTH: 3.5,
+                    Map.LANE_NUM: 3,
+                }
+            }
+        )
+
+
+def test_save_episode(vis=False):
+    setup_logger(True)
+
+    test_dump = False
+
+    env = TestEnv(vis=vis)
+    try:
+        o = env.reset()
+        epi_info = None
+        for i in range(1, 100000 if vis else 2000):
+            o, r, d, info = env.step([0, 1])
+            if vis:
+                env.render()
+            if d:
+                epi_info = env.scene_manager.dump_episode()
+
+                # test dump json
+                if test_dump:
+                    with open("test.json", "w") as f:
+                        json.dump(epi_info, f)
+                break
+    finally:
+        env.close()
+
+    del env
+    env = TestEnv(False, vis=vis)
+    try:
+        o = env.reset(epi_info)
+        for i in range(1, 100000 if vis else 2000):
+            o, r, d, info = env.step([0, 1])
+            if vis:
+                env.render()
+            if d:
+                break
+    finally:
+        env.close()
+
+
+if __name__ == "__main__":
+    test_save_episode(vis=True)

pgdrive/tests/test_top_down_rendering/test_top_down_rendering.py

@@ -0,0 +1,19 @@
+from pgdrive.envs.pgdrive_env import PGDriveEnv
+import pygame
+
+
+def test_top_down_rendering():
+    env = PGDriveEnv(dict(environment_num=20, start_seed=0, map=10, use_topdown=True, use_render=False))
+    try:
+        env.reset()
+        for i in range(5):
+            env.step(env.action_space.sample())
+            env.render(mode="human")
+            env.render(mode="rgb_array")
+    finally:
+        pygame.image.save(env.pg_world.highway_render.frame_surface, "save_offscreen.jpg")
+        env.close()
+
+
+if __name__ == "__main__":
+    test_top_down_rendering()

pgdrive/world/highway_render/highway_render.py

@@ -1,4 +1,5 @@
 import os
+import sys
 from typing import List, Tuple
 
 import numpy as np
@@ -47,28 +48,26 @@ def __init__(self, onscreen: bool, main_window_position=None):
         # main_window_position means the left upper location.
         os.environ['SDL_VIDEO_WINDOW_POS'] = \
             '%i,%i' % (main_window_position[0] - self.RESOLUTION[0], main_window_position[1])
-        self.screen = pygame.display.set_mode(self.resolution)
+        self.screen = pygame.display.set_mode(self.resolution) if onscreen else None
         self.clock = pygame.time.Clock()
 
         self.surface = WorldSurface(self.MAP_RESOLUTION, 0, pygame.Surface(self.MAP_RESOLUTION))
         self.frame_surface = pygame.Surface(self.RESOLUTION)
 
     def render(self) -> np.ndarray:
-        # for event in pygame.event.get():
-        #     if event.type == pygame.KEYDOWN:
-        #         if event.key == pygame.K_j:
-        #             self.CAM_REGION -= 5 if 10 < self.CAM_REGION else 0
-        #         if event.key == pygame.K_k:
-        #             self.CAM_REGION += 5 if self.CAM_REGION < 100 else 0
-        #         if event.key == pygame.K_ESCAPE:
-        #             sys.exit()
+        if self.onscreen:
+            for event in pygame.event.get():
+                if event.type == pygame.KEYDOWN:
+                    if event.key == pygame.K_ESCAPE:
+                        sys.exit()
 
         self.draw_scene()
-        self.screen.fill(pygame.Color("black"))
-        self.screen.blit(self.frame_surface, (0, 0))
-        # if self.clock is not None:
-        #     self.clock.tick(self.FPS)
-        pygame.display.flip()
+        if self.onscreen:
+            self.screen.fill(pygame.Color("black"))
+            self.screen.blit(self.frame_surface, (0, 0))
+            # if self.clock is not None:
+            #     self.clock.tick(self.FPS)
+            pygame.display.flip()
 
     def set_scene_mgr(self, scene_mgr):
         self.scene_mgr = scene_mgr
@@ -183,7 +182,7 @@ class VehicleGraphics(object):
     EGO_COLOR = GREEN
 
     @classmethod
-    def display(cls, vehicle, surface, offscreen: bool = False, label: bool = False) -> None:
+    def display(cls, vehicle, surface, label: bool = False) -> None:
         """
         Display a vehicle on a pygame surface.
 
@@ -219,10 +218,6 @@ def display(cls, vehicle, surface, offscreen: bool = False, label: bool = False)
         else:
             h = v.heading_theta if abs(v.heading_theta) > 2 * np.pi / 180 else 0
         position = [*surface.pos2pix(v.position[0], v.position[1])]
-        if not offscreen:
-            # convert_alpha throws errors in offscreen mode
-            # see https://stackoverflow.com/a/19057853
-            vehicle_surface = pygame.Surface.convert_alpha(vehicle_surface)
         cls.blit_rotate(surface, vehicle_surface, position, np.rad2deg(-h))
 
         # Label
@@ -437,38 +432,3 @@ def draw_ground(cls, lane, surface, color: Tuple[float], width: float, draw_surf
             new_dots = reversed(new_dots) if side else new_dots
             dots.extend(new_dots)
         pygame.draw.polygon(draw_surface, color, dots, 0)
-
-
-class RoadGraphics(object):
-    """A visualization of a road lanes."""
-    @staticmethod
-    def display(road, surface):
-        """
-        Display the road lanes on a surface.
-
-        :param road: the road to be displayed
-        :param surface: the pygame surface
-        """
-        surface.fill(surface.GREY)
-        for _from in road.network.graph.keys():
-            for _to in road.network.graph[_from].keys():
-                for l in road.network.graph[_from][_to]:
-                    LaneGraphics.display(l, surface)
-
-    @staticmethod
-    def display_traffic(road, surface, simulation_frequency: int = 15,
-                        offscreen: bool = False) \
-            -> None:
-        """
-        Display the road vehicles on a surface.
-
-        :param road: the road to be displayed
-        :param surface: the pygame surface
-        :param simulation_frequency: simulation frequency
-        :param offscreen: render without displaying on a screen
-        """
-        if road.record_history:
-            for v in road.vehicles:
-                VehicleGraphics.display_history(v, surface, simulation=simulation_frequency, offscreen=offscreen)
-        for v in road.vehicles:
-            VehicleGraphics.display(v, surface, offscreen=offscreen)