Simulator with obstacles along the path

pydrivingsim/__init__.py

@@ -7,6 +7,7 @@
 # The Object
 from pydrivingsim.vehicle import Vehicle
 from pydrivingsim.trafficlight import TrafficLight
+from pydrivingsim.obstacle import Obstacle
 
 # The Agent
 from pydrivingsim.agent import Agent

pydrivingsim/agent.py

@@ -8,8 +8,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
-
+from pydrivingsim import World, Vehicle, TrafficLight, Obstacle
 
 c = agent_lib.AgentConnector()
 
@@ -50,7 +49,7 @@ def __init__(self, vehicle: Vehicle):
         self.manoeuvre_msg_pointer = ct.pointer(self.manoeuvre_msg)
 
         self.cycle_number = 0
-        self.requested_cruising_speed = 20
+        self.requested_cruising_speed = 15
         self.action = (0,0)
 
         # Data to be filtered
@@ -114,26 +113,43 @@ def __compute(self, v :Vehicle):
             s.LatOffsLineR = -(s.LaneWidth - road_width/4) + v.state[1]
             s.LatOffsLineL = road_width/4 + v.state[1]
 
+        # Objects parameters (traffic light and obstacles)
+        trafficlight = 0
+        objn = 0
         for obj in World().obj_list:
+
             if type(obj) is TrafficLight:
                 trafficlight = obj
-                break
+                if trafficlight.pos[0] - v.state[0] > -1.5:
+                    s.NrTrfLights = 1
+                    s.TrfLightDist = trafficlight.pos[0] - v.state[0]
+                    s.TrfLightCurrState = trafficlight.state+1        # 1 = Green, 2 = Yellow, 3 = Red, 0 = Flashing
+                    s.TrfLightFirstTimeToChange = trafficlight.time_phases[trafficlight.state]-trafficlight.time_past_switch
+                    s.TrfLightFirstNextState = divmod(trafficlight.state+1,3)[1]+1
+                    s.TrfLightSecondTimeToChange = s.TrfLightFirstTimeToChange+trafficlight.time_phases[divmod(trafficlight.state+1,3)[1]]
+                    s.TrfLightSecondNextState = divmod(trafficlight.state+2,3)[1]+1
+                    s.TrfLightThirdTimeToChange = s.TrfLightSecondTimeToChange+trafficlight.time_phases[divmod(trafficlight.state+2,3)[1]]
+                else:
+                    s.NrTrfLights = 0
+                    s.TrfLightDist = trafficlight.pos[0] - v.state[0]
+                    if trafficlight.pos[0] - v.state[0] < -10.0:
+                        s.Status = 1
+
+            if type(obj) is Obstacle:
+                s.ObjX[objn] = obj.pos[0]
+                s.ObjY[objn] = obj.pos[1]
+                s.ObjVel[objn] = obj.vel
+                s.ObjLen[objn] = obj.lenght
+                s.ObjWidth[objn] = obj.width
+                objn = objn + 1
+                s.ObjID[0] = 1   # ObjID[0] is used to notify the agent if there are obstacles or not
+            else:
+                s.ObjID[0] = 0
+
+        s.NrObjs = objn
 
-        # Trafficlight parameters
-        if trafficlight.pos[0] - v.state[0] > -1.5:
-            s.NrTrfLights = 1
-            s.TrfLightDist = trafficlight.pos[0] - v.state[0]
-            s.TrfLightCurrState = trafficlight.state+1        # 1 = Green, 2 = Yellow, 3 = Red, 0 = Flashing
-            s.TrfLightFirstTimeToChange = trafficlight.time_phases[trafficlight.state]-trafficlight.time_past_switch
-            s.TrfLightFirstNextState = divmod(trafficlight.state+1,3)[1]+1
-            s.TrfLightSecondTimeToChange = s.TrfLightFirstTimeToChange+trafficlight.time_phases[divmod(trafficlight.state+1,3)[1]]
-            s.TrfLightSecondNextState = divmod(trafficlight.state+2,3)[1]+1
-            s.TrfLightThirdTimeToChange = s.TrfLightSecondTimeToChange+trafficlight.time_phases[divmod(trafficlight.state+2,3)[1]]
-        else:
-            s.NrTrfLights = 0
-            if trafficlight.pos[0] - v.state[0] < -20.0:
-                s.Status = 1
 
+        # Trafficlight parameters
         # print("CS:" + str(s.TrfLightDist))
         # print("CS:" + str(s.TrfLightCurrState))
         # print("NS:(" + str(s.TrfLightFirstTimeToChange) + "," + str(s.TrfLightFirstNextState) + ")")
@@ -166,4 +182,4 @@ def terminate(self):
         c.client_agent_close()
 
     def get_action(self):
-        return self.action
+        return self.action  

pydrivingsim/object.py

@@ -27,6 +27,9 @@ def world_freq_compute(self):
     def object_freq_compute(self):
         pass
 
+    def reset(self):
+        pass
+
     def render(self):
         pass
 

pydrivingsim/obstacle.py

@@ -0,0 +1,52 @@
+import pygame, math
+import numpy as np
+
+from pydrivingsim import VirtualObject, World
+
+class ObstacleSprite(pygame.sprite.Sprite):
+    def __init__(self, obstacle):
+        super().__init__()
+        self.obstacle = obstacle
+        image = pygame.image.load("imgs/cone.png").convert_alpha()
+        w, h = image.get_size()
+        scale = (World().scaling_factor * 1.5) / w
+        self.image_fix = pygame.transform.smoothscale(image, (int(w * scale), int(h * scale)))
+
+        self.image = self.image_fix
+        self.rect = self.image_fix.get_rect()
+        self.size = self.image_fix.get_size()
+
+    def update(self) -> None:
+        self.rect.center = [
+            self.obstacle.pos[0] * World().scaling_factor - World().get_world_pos()[0],
+            self.obstacle.pos[1] * World().scaling_factor - World().get_world_pos()[1] - self.size[1]/2
+        ]
+        self.image = self.image_fix
+
+class Obstacle(VirtualObject):
+    __metadata = {
+        "dt": 0.1
+    }
+    def __init__( self ):
+        super().__init__(self.__metadata["dt"])
+        # Sprite
+        self.obstacle = ObstacleSprite(self)
+        self.group = pygame.sprite.Group()
+        self.group.add(self.obstacle)
+        self.pos = (0,0)
+        self.lenght = 1.5
+        self.width = 1
+        self.vel = 0
+
+        self.clock = None
+        self.reset()
+
+    def set_pos(self, point: tuple):
+        self.pos = point
+
+    def render( self ):
+        self.obstacle.update()
+        self.group.draw(World().screen)
+
+    # def get_state(self):
+    #     return self.state

simulator.py

@@ -4,7 +4,7 @@
 
 import signal
 
-from pydrivingsim import World, Vehicle, TrafficLight, Agent
+from pydrivingsim import World, Vehicle, TrafficLight, Agent, Obstacle
 
 class GracefulKiller:
   kill_now = False
@@ -16,12 +16,24 @@ def exit_gracefully(self, *args):
     self.kill_now = True
 
 def main():
+
+    mode = 0      # mode settings: 0 = basic agent, 1 = obstacles
+
     vehicle = Vehicle()
     vehicle.set_screen_here()
     agent = Agent(vehicle)
     trafficlight = TrafficLight()
     trafficlight.set_pos((162,-2))
 
+    if mode == 1:
+      # Obstacles position setting
+      obs = Obstacle()
+      obs.set_pos((50,0.5))
+      obs = Obstacle()
+      obs.set_pos((90,2.5))
+      obs = Obstacle()
+      obs.set_pos((130,0.5))
+
     killer = GracefulKiller()
     while not killer.kill_now and World().loop:
         agent.compute()