diff --git a/imgs/Scenario_1.png b/imgs/Scenario_1.png
new file mode 100644
index 0000000..179dc6d
Binary files /dev/null and b/imgs/Scenario_1.png differ
diff --git a/imgs/Scenario_2.png b/imgs/Scenario_2.png
new file mode 100644
index 0000000..4d0ba4c
Binary files /dev/null and b/imgs/Scenario_2.png differ
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..eba0d8e 100644
--- a/pydrivingsim/__init__.py
+++ b/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
\ No newline at end of file
diff --git a/pydrivingsim/agent.py b/pydrivingsim/agent.py
index ba78d22..236a6d7 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, 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
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/graph.py b/pydrivingsim/graph.py
new file mode 100644
index 0000000..f5483e3
--- /dev/null
+++ b/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))
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..cd1bee8 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, GPS, DrawPath, Scenario1, Scenario2
\ No newline at end of file
diff --git a/scenarios/main_scenarios.py b/scenarios/main_scenarios.py
index 0c615ae..a7d950f 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, Graph
 
 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()
@@ -45,19 +41,56 @@ def update(self):
     def terminate(self):
         self.agent.terminate()
 
-
-class BasicTrafficLight():
+# draw path into m.m.TrajectoryPointIX
+class DrawPath():
     def __init__(self):
+        self.graph = Graph()
+
+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((182, -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 +126,96 @@ 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 Scenario1():
+    def __init__(self, av):
+        # draw the background image
+        World().set_background("imgs/Scenario_1.png", bg_pos=(-1100,-1745))
+        
+        # draw the rectangle of terrain
+        segm = RoadSegment(x=0, y=0, length=6, width=100, terrain_type="asphalt")
+        segm = RoadSegment(x=33, y=46, length=60, width=8, terrain_type="asphalt")
+        segm = RoadSegment(x=66, y=0, length=6, width=100, terrain_type="asphalt")
+        segm = RoadSegment(x=99, y=-46, length=60, width=8, terrain_type="asphalt")
+        segm = RoadSegment(x=132, y=0, length=6, width=100, terrain_type="dirt")
+        segm = RoadSegment(x=165, y=46, length=60, width=8, 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, -40, 1.57)) #((0,-1,0.1))
+        
+        #Initialize target
+        target = Target()
+        target.set_pos((175, 46))
+        target.set_object(av.vehicle)
+        
+        # draw the cones
+        cone = TrafficCone()
+        cone.set_pos((63.5, 10))
+        cone = TrafficCone()
+        cone.set_pos((65, 10))
+        cone = TrafficCone()
+        cone.set_pos((67, -20))
+        cone = TrafficCone()
+        cone.set_pos((68.5, -20))
+        
+        # draw the rocks
+        rock = Rock()
+        rock.set_pos_size((130, -5), 2.0, 2.0)
+        rock = Rock()
+        rock.set_pos_size((134, 15), 2.0, 3.0)
+        
+class Scenario2():
+    def __init__(self, av):
+        # draw the background image
+        World().set_background("imgs/Scenario_2.png", bg_pos=(-1100,-1745))
+        
+        # draw the rectangle of terrain
+        segm = RoadSegment(x=0, y=12, length=8, width=20, terrain_type="asphalt")
+        segm = RoadSegment(x=14, y=18, length=20, width=8, terrain_type="asphalt")
+        segm = RoadSegment(x=13, y=0, length=42, width=4, terrain_type="asphalt")
+        segm = RoadSegment(x=30, y=-7, length=8, width=10, terrain_type="asphalt")
+        segm = RoadSegment(x=54, y=-10, length=40, width=4, terrain_type="asphalt")
+        segm = RoadSegment(x=78, y=-7, length=8, width=10, terrain_type="asphalt")
+        segm = RoadSegment(x=54, y=0, length=40, width=4, terrain_type="dirt")
+        segm = RoadSegment(x=134, y=0, length=120, width=4, terrain_type="asphalt")
+        segm = RoadSegment(x=162.5, y=0, length=4, width=40, 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((14, 18, 3.14)) #((0,-1,0.1))
+        
+        #Initialize target
+        target = Target()
+        target.set_pos((182, -1))
+        target.set_object(av.vehicle)
+        
+        # draw the cones
+        cone = TrafficCone()
+        cone.set_pos((0, 0))
+        cone = TrafficCone()
+        cone.set_pos((65, -8))
+        cone = TrafficCone()
+        cone.set_pos((65, -9))
+        cone = TrafficCone()
+        cone.set_pos((90, 0.5))
+        cone = TrafficCone()
+        cone.set_pos((90, 1.5))
+
+        # draw the traffic light
+        trafficlight = TrafficLight()
+        trafficlight.set_pos((160,-3))
+        trafficlight.reset()
+        
+        # draw the rocks
+        rock = Rock()
+        rock.set_pos_size((60, -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..c1039b3 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, DrawPath, Scenario1, Scenario2
 
 class GracefulKiller:
   kill_now = False
@@ -17,14 +17,16 @@ def exit_gracefully(self, *args):
     self.kill_now = True
 
 def main():
+    # Enable this to draw path into m.m.TrajectoryPointIX
+    #DrawPath()
     # 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
+    #Scenario1(av) #zig-zag road with no TL to test lateral control
+    #Scenario2(av) #scenario with multiple paths
 
     killer = GracefulKiller()
     while not killer.kill_now and World().loop: