Add scenario 2

pydrivingsim/__init__.py

@@ -13,5 +13,11 @@
 from pydrivingsim.suggestedspeedsignal import SuggestedSpeedSignal
 from pydrivingsim.graphicobject import GraphicObject
 
+from pydrivingsim.graph import Graph
+from pydrivingsim.rock import Rock
+from pydrivingsim.gps import GPS
+from pydrivingsim.roadsegment import RoadSegment
+from pydrivingsim.world import World
+
 # The Agent
 from pydrivingsim.agent import Agent

pydrivingsim/agent.py

@@ -9,7 +9,7 @@
 import agent.agent_interfaces_connector as agent_lib
 from agent.interfaces_python_data_structs import input_data_str, output_data_str
 
-from pydrivingsim import World, Vehicle, TrafficLight, TrafficCone, Target, SuggestedSpeedSignal, Coin
+from pydrivingsim import World, Vehicle, TrafficLight, TrafficCone, Target, SuggestedSpeedSignal, Coin, Rock, RoadSegment, GPS, Graph
 
 
 c = agent_lib.AgentConnector()
@@ -58,6 +58,12 @@ def __init__(self, vehicle: Vehicle):
         self.ALgtFild = 0
         self.YawRateFild = 0
         self.SteerWhlAg = 0
+
+        # find Graph in the world
+        for obj in World().obj_list:
+            if isinstance(obj, Graph):
+                self.graph = obj
+                break
 
     def compute(self):
         self.num_of_step += 1
@@ -99,7 +105,7 @@ def __compute(self, v :Vehicle):
         # Vehicle parameters
         s.VehicleLen = v.vehicle.vehicle.L                          # double - lenght dimension [m]
         s.VehicleWidth = v.vehicle.vehicle.Wf                       # double - width dimension [m]
-        s.LaneHeading = -v.state[2]
+        s.LaneHeading = v.state[2]
         #print(v.state[2])
         #print((v.state[0],v.state[1]))
         s.VLgtFild = v.state[3]
@@ -155,6 +161,48 @@ def __compute(self, v :Vehicle):
                 s.AdasisSpeedLimitValues[speedlimitId] = obj.vel
                 s.AdasisSpeedLimitDist[speedlimitId] = obj.pos[0] - v.state[0]
                 speedlimitId = speedlimitId + 1
+
+            # Code for sending RockObstacle information
+            if type(obj) is Rock:
+                s.ObjID[objId] = 3
+                delta_x = obj.pos[0] - v.state[0]
+                delta_y = obj.pos[1] - v.state[1]
+                s.ObjX[objId] = delta_x * cos(v.state[2]) + delta_y * sin(v.state[2])
+                s.ObjY[objId] = - delta_x * sin(v.state[2]) + delta_y * cos(v.state[2])
+                s.ObjVel[objId] = 0
+                s.ObjLen[objId] = obj.len
+                s.ObjWidth[objId] = obj.width
+                objId = objId + 1
+
+            # Code for sending target information
+            if type(obj) is Target:
+                s.ObjID[objId] = 5
+                delta_x = obj.pos[0] - v.state[0]
+                delta_y = obj.pos[1] - v.state[1]
+                s.ObjX[objId] = delta_x * cos(v.state[2]) + delta_y * sin(v.state[2])
+                s.ObjY[objId] = - delta_x * sin(v.state[2]) + delta_y * cos(v.state[2])
+                s.ObjVel[objId] = 0
+                objId = objId + 1
+
+            # Code for sending RoadSegment information
+            if type(obj) is RoadSegment:
+                s.ObjID[objId] = obj.type_id  # 4=Asphalt; 6=Dirt
+                delta_x = obj.pos[0] - v.state[0]
+                delta_y = obj.pos[1] - v.state[1]
+                s.ObjX[objId] = delta_x * cos(v.state[2]) + delta_y * sin(v.state[2])
+                s.ObjY[objId] = - delta_x * sin(v.state[2]) + delta_y * cos(v.state[2])
+                s.ObjVel[objId] = 0
+                s.ObjLen[objId] = obj.length
+                s.ObjWidth[objId] = obj.width
+                objId = objId + 1
+
+            # Code for GPS information
+            if type(obj) is GPS:
+                s.ObjID[objId] = 99
+                s.ObjX[objId] = v.state[0]
+                s.ObjY[objId] = v.state[1]
+                s.ObjVel[objId] = 0
+                objId = objId + 1
 
         s.NrObjs = objId
         s.AdasisSpeedLimitNr = speedlimitId
@@ -191,6 +239,21 @@ def __compute(self, v :Vehicle):
 
         #self.action = (0.01, 0.01)
         self.action = (m.RequestedAcc,m.RequestedSteerWhlAg)
+
+        # Save values from manoeuvre_msg into the Graph class
+        if hasattr(self, "graph") and self.graph is not None:
+            if m.NTrajectoryPoints > 0:
+                #print("Received points: " + str(m.NTrajectoryPoints))
+                points_world = []
+                for i in range(m.NTrajectoryPoints):
+                    xr = m.TrajectoryPointIX[i]
+                    yr = m.TrajectoryPointIY[i]
+
+                    #print("xr: " + str(xr))
+                    #print("yr: " + str(yr))
+                    points_world.append((xr, yr))
+
+                self.graph.set_points_world(points_world)
 
     def terminate(self):
         World().loop = 0

pydrivingsim/gps.py

@@ -0,0 +1,20 @@
+import pygame
+import random
+
+from pydrivingsim import VirtualObject
+
+class GPS(VirtualObject):
+    __metadata = {
+        "dt": 0.1
+    }
+    def __init__(self, vehicle):
+        super().__init__(self.__metadata["dt"])
+        self.vehicle = vehicle
+        self.x = 0.0
+        self.y = 0.0
+        self.psi = 0.0
+
+    def update(self):
+        self.x = self.vehicle.state[0]
+        self.y = self.vehicle.state[1]
+        self.psi = self.vehicle.state[2]

pydrivingsim/graph.py

@@ -0,0 +1,81 @@
+import pygame
+import math
+
+from pydrivingsim import VirtualObject, World
+
+class GraphNodeSprite(pygame.sprite.Sprite):
+    def __init__(self, graph, index):
+        super().__init__()
+        self.graph = graph    # graph containing 23 nodes
+        self.index = index    # index of the node [0..22]
+
+        # draw one node
+        radius = 3
+        size = radius * 2 + 2
+        self.image = pygame.Surface((size, size), pygame.SRCALPHA)
+        pygame.draw.circle(self.image, (255, 0, 0), (size // 2, size // 2), radius)
+        self.rect = self.image.get_rect()
+
+    def update(self) -> None:
+        # disable sprite if there are not enough points
+        if self.index >= len(self.graph.points_world):
+            self.rect.center = (-1000, -1000) #center out of the screen
+            return
+
+        Xw, Yw = self.graph.points_world[self.index]
+
+        # transform world -> screen
+        world = World()
+        X_ref, Y_ref = world.get_world_pos()
+        s  = world.scaling_factor
+        cx = world.screen_world_center[0]
+        cy = world.screen_world_center[1]
+
+        x_screen = (Xw - X_ref) * s + cx
+        y_screen = (Y_ref - Yw) * s + cy
+
+        self.rect.center = (x_screen, y_screen)
+
+
+class Graph(VirtualObject):
+    __metadata = { "dt": 0.1 }
+
+    def __init__(self):
+        super().__init__(self.__metadata["dt"])
+
+        self.points_world = []
+
+        # group of nodes
+        self.group = pygame.sprite.Group()
+        self.max_points = 23
+        for i in range(self.max_points):
+            sprite = GraphNodeSprite(self, i)
+            self.group.add(sprite)
+
+    def set_points_world(self, points):
+        self.points_world = list(points[:self.max_points])
+
+    def render(self):
+        # draw circles
+        self.group.update()
+        self.group.draw(World().screen)
+
+        # draw arcs
+        if len(self.points_world) >= 2:
+            world = World()
+            screen = world.screen
+            X_ref, Y_ref = world.get_world_pos()
+            s  = world.scaling_factor
+            cx = world.screen_world_center[0]
+            cy = world.screen_world_center[1]
+
+            pixel_points = []
+            for (Xw, Yw) in self.points_world:
+                x_screen = (Xw - X_ref) * s + cx
+                y_screen = (Y_ref - Yw) * s + cy
+                pixel_points.append((x_screen, y_screen))
+
+            for i in range(len(pixel_points) - 1):
+                x1, y1 = pixel_points[i]
+                x2, y2 = pixel_points[i+1]
+                pygame.draw.aaline(screen, (200, 0, 0), (x1, y1), (x2, y2))

pydrivingsim/roadsegment.py

@@ -0,0 +1,55 @@
+import pygame
+from pydrivingsim import VirtualObject, World
+
+class RoadSegmentSprite(pygame.sprite.Sprite):
+    def __init__(self, road_segment):
+        super().__init__()
+
+        # draw the roadSegment
+        # for now, it just draws rectangles -> to do: implement texture
+        if road_segment.type_id == 7: # Dirt
+            color = (139, 69, 19) 
+        else: # Asphalt (ID 6)
+            color = (80, 80, 80)
+
+        self.road_segment = road_segment
+
+        w_pixel = int(World().scaling_factor * road_segment.length)
+        h_pixel = int(World().scaling_factor * road_segment.width)
+
+        self.image = pygame.Surface([w_pixel, h_pixel])
+        self.image.fill(color)
+
+        self.image.set_alpha(200) 
+
+        self.rect = self.image.get_rect()
+
+    def update(self) -> None:
+        self.rect.center = [
+            (self.road_segment.pos[0] - World().get_world_pos()[0]) * World().scaling_factor + World().screen_world_center[0],
+            (World().get_world_pos()[1] - self.road_segment.pos[1]) * World().scaling_factor + World().screen_world_center[1]
+        ]
+
+class RoadSegment(VirtualObject):
+    __metadata = {
+        "dt": 0.1
+    }
+    def __init__(self, x, y, length, width, terrain_type="asphalt"):
+        super().__init__(self.__metadata["dt"])
+        self.pos = (x, y)
+        self.length = length
+        self.width = width
+
+        if terrain_type == "dirt":
+            self.type_id = 7
+        else:
+            self.type_id = 6
+
+        self.sprite = RoadSegmentSprite(self)
+        self.group = pygame.sprite.Group()
+        self.group.add(self.sprite)
+
+    # decomment if you want the roadSegments to be drawn
+    #def render(self):
+        #self.sprite.update()
+        #self.group.draw(World().screen)

pydrivingsim/rock.py

@@ -0,0 +1,83 @@
+import pygame
+import random
+
+from pydrivingsim import VirtualObject, World
+
+class RockSprite(pygame.sprite.Sprite):
+    def __init__(self, rock_obstacle):
+        super().__init__()
+        img = "imgs/rock.png"
+        self.image_fix = []
+        self.sprite = pygame.image.load(img).convert_alpha()
+        w, h = self.sprite.get_size()
+        scale_x = (World().scaling_factor * rock_obstacle.len) / w
+        scale_y = (World().scaling_factor * rock_obstacle.width) / h
+
+        new_w = max(1, int(w * scale_x))
+        new_h = max(1, int(h * scale_y))
+
+        self.image_fix.append(
+            pygame.transform.smoothscale(self.sprite, (new_w, new_h))
+        )
+        self.image = self.image_fix[0]
+        self.rect = self.image_fix[0].get_rect()
+        self.size = self.image_fix[0].get_size()
+        self.rock_obstacle = rock_obstacle
+
+    def resize(self, len, width):
+        w, h = self.sprite.get_size()
+        s = World().scaling_factor
+
+        scale_x = (s * len) / w
+        scale_y = (s * width) / h
+
+        new_w = max(1, int(w * scale_x))
+        new_h = max(1, int(h * scale_y))
+
+        scaled = pygame.transform.smoothscale(self.sprite, (new_w, new_h))
+
+        if self.image_fix:
+            self.image_fix[0] = scaled
+        else:
+            self.image_fix.append(scaled)
+
+        self.image = self.image_fix[0]
+        self.rect = self.image.get_rect()
+        self.size = self.image.get_size()
+
+    def update(self) -> None:
+        self.rect.center = [
+            (self.rock_obstacle.pos[0] - World().get_world_pos()[0]) * World().scaling_factor + World().screen_world_center[0],
+            (World().get_world_pos()[1] - self.rock_obstacle.pos[1]) * World().scaling_factor + World().screen_world_center[1]
+        ]
+        self.image = self.image_fix[self.rock_obstacle.state]
+
+class Rock(VirtualObject):
+    __metadata = {
+        "dt": 0.1
+    }
+    def __init__( self ):
+        super().__init__(self.__metadata["dt"])
+        # Sprite
+        self.size = 1 # 0.5
+        self.state = 0
+        self.pos = (0,0)
+        self.len = 1.0
+        self.width = 1.0
+        self.rock = RockSprite(self)
+        self.group = pygame.sprite.Group()
+        self.group.add(self.rock)
+        self.reset()
+
+    def set_pos_size(self, point, len: float, width: float):
+        self.pos = point
+        self.len = len
+        self.width = width
+        self.rock.resize(len, width)
+
+    def reset(self):
+        self.state = 0
+
+    def render( self ):
+        self.rock.update()
+        self.group.draw(World().screen)

pydrivingsim/world.py

@@ -91,4 +91,10 @@ def update(self):
     def exit(self):
         pygame.display.quit()
         pygame.quit()
-        exit()
+        exit()
+
+    # Added
+    def set_background(self, image_path, bg_pos=None):
+        self.backgorund = pygame.image.load(image_path)
+        if bg_pos:
+            self.bg_pos = bg_pos

scenarios/__init__.py

@@ -1,2 +1,2 @@
 # The basic scenarios
-from scenarios.main_scenarios import AutonomousVehicle, OnlyVehicle, BasicSpeedLimit, BasicTrafficLight, GetTheCoins
+from scenarios.main_scenarios import AutonomousVehicle, OnlyVehicle, BasicSpeedLimit, Scenario_BasicTL, GetTheCoins, GPS, DrawPath, Scenario1, Scenario2