Add gps

pydrivingsim/__init__.py

@@ -13,5 +13,10 @@
 from pydrivingsim.suggestedspeedsignal import SuggestedSpeedSignal
 from pydrivingsim.graphicobject import GraphicObject
 
+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
 
 
 c = agent_lib.AgentConnector()
@@ -99,7 +99,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 +155,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

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/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, ObstacleRocks, GPS

scenarios/main_scenarios.py

@@ -1,5 +1,5 @@
 
-from pydrivingsim import TrafficLight, Target, TrafficCone, SuggestedSpeedSignal, GraphicObject, Vehicle, Agent, Coin
+from pydrivingsim import TrafficLight, Target, TrafficCone, SuggestedSpeedSignal, GraphicObject, Vehicle, Agent, Coin, Rock, RoadSegment, World, GPS
 
 class OnlyVehicle():
     def __init__(self):
@@ -26,15 +26,11 @@ def __init__(self):
         self.vehicle = Vehicle()
         self.vehicle.set_screen_here()
         self.vehicle.set_pos_ang((0, -1, 0))
+        self.gps = GPS(self.vehicle)
 
         #Initialize the agent
         self.agent = Agent(self.vehicle)
 
-        #Initialize target
-        target = Target()
-        target.set_pos((182, -1))
-        target.set_object(self.vehicle)
-
     def update(self):
         self.agent.compute()
         action = self.agent.get_action()
@@ -46,18 +42,51 @@ def terminate(self):
         self.agent.terminate()
 
 
-class BasicTrafficLight():
-    def __init__(self):
+class Scenario_BasicTL():
+    def __init__(self, av):
+
+        # draw the background image
+        World().set_background("imgs/bg.jpeg", bg_pos=(-1100,-1745))
+
+        # draw the rectangle of terrain
+        # (x, y) is the CENTER of the segment
+        segm = RoadSegment(x=90, y=0, length=270, width=4, terrain_type="asphalt")
+
+        # draw the vehicle
+        # remove and add the vehicle to put it in the focus
+        if av.vehicle in World().obj_list:
+            World().obj_list.remove(av.vehicle)
+        World().obj_list.append(av.vehicle)
+        av.vehicle.set_pos_ang((0,-1,0))
+
+        #Initialize target
+        target = Target()
+        target.set_pos((220, -1))
+        target.set_object(av.vehicle)
+
+        # draw the cones
         cone = TrafficCone()
         cone.set_pos((1.0,0))
         cone = TrafficCone()
         cone.set_pos((1.0,2))
         cone = TrafficCone()
         cone.set_pos((1.0,-2))
-
+
+        # draw the rocks
+        #rock = Rock()
+        #rock.set_pos_size((1, -5), 2.0, 2.0)
+        #rock = Rock()
+        #rock.set_pos_size((1, 5), 1.0, 1.0)
+
+        # set pos of the TL
         trafficlight = TrafficLight()
         trafficlight.set_pos((160,-3))
         trafficlight.reset()
+
+        # Enable this to test the coins
+        #GetTheCoins()
+        # Enable this to test the speed limit
+        #BasicSpeedLimit()
 
 class GetTheCoins():
     def __init__(self):
@@ -93,4 +122,11 @@ def __init__(self):
         signal = SuggestedSpeedSignal(90)
         signal.set_pos((96, 4))
         super = GraphicObject("imgs/pictures/superstrada.png", 5)
-        super.set_pos((100,6))
+        super.set_pos((100,6))
+
+class ObstacleRocks():
+    def __init__(self):
+        rock = Rock()
+        rock.set_pos_size((20, -2), 2.0, 2.0)
+        rock = Rock()
+        rock.set_pos_size((45, 2), 3.0, 4.0)

simulator.py

@@ -5,7 +5,7 @@
 import signal
 
 from pydrivingsim import World
-from scenarios import BasicSpeedLimit, BasicTrafficLight, OnlyVehicle, AutonomousVehicle, GetTheCoins
+from scenarios import BasicSpeedLimit, Scenario_BasicTL, OnlyVehicle, AutonomousVehicle, GetTheCoins
 
 class GracefulKiller:
   kill_now = False
@@ -20,11 +20,9 @@ def main():
     # Enable this to test only single vehicle
     #av = OnlyVehicle()
     av = AutonomousVehicle()
-    BasicTrafficLight()
-    # Enable this to test the coins
-    #GetTheCoins()
-    # Enable this to test the speed limit
-    BasicSpeedLimit()
+
+    # choose the scenario
+    Scenario_BasicTL(av) #straight road with the traffic light
 
     killer = GracefulKiller()
     while not killer.kill_now and World().loop: