diff --git a/imgs/rock.png b/imgs/rock.png new file mode 100644 index 0000000..24346b7 Binary files /dev/null and b/imgs/rock.png differ diff --git a/pydrivingsim/__init__.py b/pydrivingsim/__init__.py index cc5509a..1c47ac0 100644 --- a/pydrivingsim/__init__.py +++ b/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 \ No newline at end of file diff --git a/pydrivingsim/agent.py b/pydrivingsim/agent.py index ba78d22..58e85b0 100644 --- a/pydrivingsim/agent.py +++ b/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 diff --git a/pydrivingsim/gps.py b/pydrivingsim/gps.py new file mode 100644 index 0000000..f7fc800 --- /dev/null +++ b/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] \ No newline at end of file diff --git a/pydrivingsim/roadsegment.py b/pydrivingsim/roadsegment.py new file mode 100644 index 0000000..6c45d2a --- /dev/null +++ b/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) \ No newline at end of file diff --git a/pydrivingsim/rock.py b/pydrivingsim/rock.py new file mode 100644 index 0000000..d47eeac --- /dev/null +++ b/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) \ No newline at end of file diff --git a/pydrivingsim/world.py b/pydrivingsim/world.py index 4bad5ab..575cd07 100644 --- a/pydrivingsim/world.py +++ b/pydrivingsim/world.py @@ -91,4 +91,10 @@ def update(self): def exit(self): pygame.display.quit() pygame.quit() - exit() \ No newline at end of file + 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 \ No newline at end of file diff --git a/scenarios/__init__.py b/scenarios/__init__.py index 2520440..a0377fb 100644 --- a/scenarios/__init__.py +++ b/scenarios/__init__.py @@ -1,2 +1,2 @@ # The basic scenarios -from scenarios.main_scenarios import AutonomousVehicle, OnlyVehicle, BasicSpeedLimit, BasicTrafficLight, GetTheCoins \ No newline at end of file +from scenarios.main_scenarios import AutonomousVehicle, OnlyVehicle, BasicSpeedLimit, Scenario_BasicTL, GetTheCoins, ObstacleRocks, GPS \ No newline at end of file diff --git a/scenarios/main_scenarios.py b/scenarios/main_scenarios.py index 0c615ae..86316e6 100644 --- a/scenarios/main_scenarios.py +++ b/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)) \ No newline at end of file + 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) \ No newline at end of file diff --git a/simulator.py b/simulator.py index c696cf7..83914a1 100644 --- a/simulator.py +++ b/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: