diff --git a/imgs/cone.png b/imgs/cone.png new file mode 100644 index 0000000..fbf297a Binary files /dev/null and b/imgs/cone.png differ diff --git a/pydrivingsim/__init__.py b/pydrivingsim/__init__.py index 1c78a3e..4eb11b1 100644 --- a/pydrivingsim/__init__.py +++ b/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 \ No newline at end of file diff --git a/pydrivingsim/agent.py b/pydrivingsim/agent.py index 826412a..053fe12 100644 --- a/pydrivingsim/agent.py +++ b/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 \ No newline at end of file diff --git a/pydrivingsim/object.py b/pydrivingsim/object.py index cdd6ed4..0cc2551 100644 --- a/pydrivingsim/object.py +++ b/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 diff --git a/pydrivingsim/obstacle.py b/pydrivingsim/obstacle.py new file mode 100644 index 0000000..43c4f39 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/simulator.py b/simulator.py index 2a07ae2..5937f43 100644 --- a/simulator.py +++ b/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()