RL_DQN-Vel-Constraint _ varying throttle.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 97,
   "id": "c8a6105d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import csv\n",
    "import carla\n",
    "import numpy as np\n",
    "import random\n",
    "import torch\n",
    "import torch.nn as nn\n",
    "import torch.optim as optim\n",
    "from torch.autograd import Variable\n",
    "import gymnasium as gym\n",
    "from gymnasium import spaces\n",
    "from collections import deque\n",
    "import copy\n",
    "import time\n",
    "import math"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 98,
   "id": "6ac07fe6",
   "metadata": {},
   "outputs": [],
   "source": [
    "class Vector:\n",
    "    def __init__(self, x, y, z):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.z = z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 99,
   "id": "34b6d1e4",
   "metadata": {},
   "outputs": [],
   "source": [
    "threshold = 2\n",
    "max_speed = 17 # m/s"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 100,
   "id": "25feaf7a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Configuration\n",
    "CSV_FILE = \"waypoint.csv\"\n",
    "EPISODES = 100\n",
    "STEPS = 50000\n",
    "BATCH_SIZE = 64\n",
    "GAMMA = 0.99\n",
    "LEARNING_RATE = 1e-3\n",
    "MEMORY_SIZE = 10000\n",
    "EPSILON_DECAY = 0.995\n",
    "MIN_EPSILON = 0.01\n",
    "old_v = 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 101,
   "id": "606b5e50",
   "metadata": {},
   "outputs": [],
   "source": [
    "class DQNAgent(nn.Module):\n",
    "    def __init__(self, state_size, action_size):\n",
    "        super(DQNAgent, self).__init__()\n",
    "        self.action_size = action_size\n",
    "        self.state_size = state_size\n",
    "        self.fc1 = nn.Linear(state_size, 64)\n",
    "        self.fc2 = nn.Linear(64, 32)\n",
    "        self.fc3 = nn.Linear(32, action_size)\n",
    "\n",
    "    def forward(self, x):\n",
    "        x = torch.relu(self.fc1(x))\n",
    "        x = torch.relu(self.fc2(x))\n",
    "        x = self.fc3(x)\n",
    "        return x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 102,
   "id": "34475080",
   "metadata": {},
   "outputs": [],
   "source": [
    "def optimize_model(agent, target_agent, memory, optimizer, batch_size, gamma):\n",
    "    if len(memory) < batch_size:\n",
    "        return\n",
    "\n",
    "    batch = random.sample(memory, batch_size)\n",
    "    state_batch, action_batch, reward_batch, next_state_batch, done_batch = zip(*batch)\n",
    "\n",
    "    state_batch = torch.cat(state_batch)\n",
    "    action_batch = torch.cat(action_batch)\n",
    "    reward_batch = torch.cat(reward_batch)\n",
    "    next_state_batch = torch.cat(next_state_batch)\n",
    "    done_batch = torch.cat(done_batch)\n",
    "\n",
    "    current_Q_values = agent(state_batch).gather(1, action_batch)\n",
    "    next_max_q = target_agent(next_state_batch).detach().max(1)[0].unsqueeze(1)\n",
    "    target_Q_values = reward_batch + (gamma * next_max_q * (1 - done_batch))\n",
    "\n",
    "    loss = nn.SmoothL1Loss()(current_Q_values, target_Q_values)\n",
    "\n",
    "    optimizer.zero_grad()\n",
    "    loss.backward()\n",
    "    optimizer.step()\n",
    "\n",
    "def epsilon_greedy_policy(agent, state, epsilon):\n",
    "    if random.random() > epsilon:\n",
    "        with torch.no_grad():\n",
    "            return agent(state).max(1)[1].view(1, 1)\n",
    "    else:\n",
    "        return torch.tensor([[random.randrange(agent.action_size)]], dtype=torch.long)\n",
    "    \n",
    "def waypoint_based_action(vehicle, next_waypoint):\n",
    "    current_transform = vehicle.get_transform()\n",
    "    current_yaw = current_transform.rotation.yaw\n",
    "\n",
    "    next_location = carla.Location(x=next_waypoint[0], y=next_waypoint[1])\n",
    "    desired_yaw = math.degrees(math.atan2(next_location.y - current_transform.location.y, \n",
    "                                          next_location.x - current_transform.location.x))\n",
    "    yaw_diff = (desired_yaw - current_yaw) % 360\n",
    "\n",
    "    if yaw_diff > 180:\n",
    "        yaw_diff -= 360\n",
    "\n",
    "    if yaw_diff > 5:\n",
    "        action = 3  # Steer right\n",
    "    elif yaw_diff < -5:\n",
    "        action = 2  # Steer left\n",
    "    else:\n",
    "        action = 0  # Go straight\n",
    "#     print(f'action decided: {action}, current yaw: {yaw_diff}')\n",
    "    return torch.tensor([[action]], dtype=torch.long)\n",
    "\n",
    "    \n",
    "\n",
    "def read_waypoints(file_path):\n",
    "    waypoints = []\n",
    "    with open(file_path, 'r') as csvfile:\n",
    "        csvreader = csv.reader(csvfile)\n",
    "        for row in csvreader:\n",
    "            x, y, yaw = map(float, row)\n",
    "            waypoints.append((x, y, yaw))\n",
    "            \n",
    "    return waypoints\n",
    "\n",
    "class CarlaEnv(gym.Env):\n",
    "    def __init__(self, vehicle, waypoints):\n",
    "        super(CarlaEnv, self).__init__()\n",
    "        self.vehicle = vehicle\n",
    "        self.waypoints = waypoints\n",
    "        self.current_waypoint_index = 0\n",
    "\n",
    "        self.action_space = spaces.Discrete(4)  # [throttle, brake, steer_left, steer_right]\n",
    "        self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(5,), dtype=np.float32)\n",
    "\n",
    "        self.lane_invasion = False\n",
    "#         print('CarlaEnv made')\n",
    "#         self.lane_invasion_sensor = self._spawn_lane_invasion_sensor()\n",
    "        \n",
    "#     def _spawn_lane_invasion_sensor(self):\n",
    "#         sensor_bp = self.world.get_blueprint_library().find('sensor.other.lane_invasion')\n",
    "#         sensor_transform = carla.Transform(carla.Location(x=2.5, z=0.7))\n",
    "#         sensor = self.world.spawn_actor(sensor_bp, sensor_transform, attach_to=self.vehicle)\n",
    "#         sensor.listen(lambda event: self._on_lane_invasion(event))\n",
    "#         return sensor\n",
    "\n",
    "    def _on_lane_invasion(self, event):\n",
    "#         print(\"lane invasion detected\")\n",
    "        self.lane_invasion = True    \n",
    "\n",
    "    def reset(self):\n",
    "        vel = carla.Vector3D()\n",
    "        vel.x = 0\n",
    "        vel.y = 0\n",
    "        vel.z = 0\n",
    "        self.vehicle.set_target_velocity(vel)\n",
    "        x, y, yaw = self.waypoints[0]\n",
    "        transform = carla.Transform(carla.Location(x, y), carla.Rotation(yaw=yaw))\n",
    "        self.vehicle.set_transform(transform)\n",
    "        v = self.vehicle.get_velocity()\n",
    "        self.vel = ((v.x)**2+(v.y)**2)**0.5\n",
    "        \n",
    "#         print(\"reset done\")\n",
    "        return self._get_observation()\n",
    "\n",
    "    def step(self, action):\n",
    "        global old_v\n",
    "        throttle = 0\n",
    "        brake = 0\n",
    "        steer = 0\n",
    "        slowing = False\n",
    "        accelarate = False\n",
    "        v = self.vehicle.get_velocity()\n",
    "        self.vel = ((v.x)**2+(v.y)**2)**0.5\n",
    "        if self.vel > max_speed:\n",
    "            slowing = True\n",
    "        else:\n",
    "            accelarate = True\n",
    "            \n",
    "        if action == 0 and slowing:\n",
    "            brake = 1.0\n",
    "        elif action ==0 and accelarate:\n",
    "            if self.vel > old_v:\n",
    "                throttle = (max_speed - old_v)/(max_speed)\n",
    "            else:\n",
    "                throttle = 1.0\n",
    "        elif action == 1:\n",
    "            brake = 1.0\n",
    "        elif action == 2:\n",
    "            steer = -1.0\n",
    "        elif action == 3:\n",
    "            steer = 1.0\n",
    "#         print (f'step taken: Throttle: {throttle} and vel: {self.vel}')\n",
    "        control = carla.VehicleControl(throttle=throttle, brake=brake, steer=steer)\n",
    "        self.vehicle.apply_control(control)\n",
    "        old_v = self.vel\n",
    "\n",
    "        next_waypoint = self.waypoints[self.current_waypoint_index]\n",
    "        next_waypoint_location = carla.Location(x=next_waypoint[0], y=next_waypoint[1])\n",
    "        current_location = self.vehicle.get_location()\n",
    "        distance = current_location.distance(next_waypoint_location)\n",
    "\n",
    "        if distance < threshold:\n",
    "            self.current_waypoint_index += 1\n",
    "            if self.current_waypoint_index >= len(self.waypoints):\n",
    "                self.current_waypoint_index = 0\n",
    "\n",
    "        reward = self._get_reward(distance, control)\n",
    "\n",
    "        done = False\n",
    "        if self.current_waypoint_index == len(self.waypoints) - 1:\n",
    "            done = True\n",
    "        return self._get_observation(), reward, done, {}\n",
    "\n",
    "    def _get_observation(self):\n",
    "        location = self.vehicle.get_location()\n",
    "        orientation = self.vehicle.get_transform().rotation.yaw\n",
    "        speed = self.vehicle.get_velocity()\n",
    "        speed = np.sqrt(speed.x**2 + speed.y**2 + speed.z**2)\n",
    "        next_waypoint = self.waypoints[self.current_waypoint_index]\n",
    "        next_waypoint_location = carla.Location(x=next_waypoint[0], y=next_waypoint[1])\n",
    "        distance = location.distance(next_waypoint_location)\n",
    "#         print('observation taken')\n",
    "        return np.array([location.x, location.y, orientation, speed, distance])\n",
    "\n",
    "\n",
    "    def _get_reward(self, distance, control):\n",
    "        reward = 0\n",
    "#         if control.throttle > 0:\n",
    "#             reward += 0\n",
    "        if distance < threshold:\n",
    "            reward += 10    \n",
    "        # Apply a large negative reward for lane invasion\n",
    "        if self.lane_invasion:\n",
    "            reward -= 1000  # Adjust the value as needed\n",
    "            self.lane_invasion = True    \n",
    "#         print('reward calculated')    \n",
    "        return reward"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 103,
   "id": "19868d0d",
   "metadata": {},
   "outputs": [],
   "source": [
    "scores = []\n",
    "def main():\n",
    "    # Set up RL agent\n",
    "    state_size = 5\n",
    "    action_size = 4\n",
    "    agent = DQNAgent(state_size, action_size)\n",
    "    target_agent = copy.deepcopy(agent)\n",
    "    optimizer = optim.Adam(agent.parameters(), lr=LEARNING_RATE)\n",
    "    memory = deque(maxlen=MEMORY_SIZE)\n",
    "    epsilon = 1.0\n",
    "    scores = []\n",
    "    try:\n",
    "        # Read waypoints from CSV\n",
    "        waypoints = read_waypoints(CSV_FILE)\n",
    "\n",
    "        # Connect to CARLA server\n",
    "        client = carla.Client('localhost', 2000)\n",
    "        client.set_timeout(10.0)\n",
    "\n",
    "        # Load CARLA world\n",
    "        world = client.get_world()\n",
    "        world.set_weather(carla.WeatherParameters.ClearNoon)\n",
    "        blueprint_library = world.get_blueprint_library()\n",
    "\n",
    "        # Spawn vehicle\n",
    "        vehicle_bp = random.choice(blueprint_library.filter('wrangler_rubicon'))\n",
    "        spawn_point = carla.Transform(carla.Location(x=-23.6,y=137.5,z=1),carla.Rotation(yaw=0))\n",
    "        vehicle = world.spawn_actor(vehicle_bp, spawn_point)\n",
    "\n",
    "        sensor_bp = world.get_blueprint_library().find('sensor.other.lane_invasion')\n",
    "        sensor_transform = carla.Transform(carla.Location(x=2.5, z=0.7))\n",
    "        sensor = world.spawn_actor(sensor_bp, sensor_transform, attach_to=vehicle)\n",
    "        sensor.listen(lambda event: env._on_lane_invasion(event))\n",
    "\n",
    "        # Create CARLA environment\n",
    "        env = CarlaEnv(vehicle, waypoints)\n",
    "\n",
    "        # Train the DQN agent\n",
    "        for episode in range(EPISODES):\n",
    "            state = env.reset()\n",
    "            state = torch.tensor(state, dtype=torch.float32).unsqueeze(0)\n",
    "            episode_reward = 0\n",
    "            env.current_waypoint_index = 0\n",
    "            env.lane_invasion = False\n",
    "            for _ in range(STEPS):\n",
    "#                 action = epsilon_greedy_policy(agent, state, epsilon)\n",
    "                \n",
    "                next_waypoint = env.waypoints[env.current_waypoint_index]\n",
    "                action = waypoint_based_action(vehicle, next_waypoint)\n",
    "\n",
    "                \n",
    "                next_state, reward, done, _ = env.step(action.item())\n",
    "                next_state = torch.tensor(next_state, dtype=torch.float32).unsqueeze(0)\n",
    "\n",
    "                memory.append((state, action, torch.tensor([reward], dtype=torch.float32).unsqueeze(0),\n",
    "                               next_state, torch.tensor([done], dtype=torch.float32).unsqueeze(0)))\n",
    "\n",
    "                state = next_state\n",
    "                episode_reward += reward\n",
    "                epsilon = max(epsilon * EPSILON_DECAY, MIN_EPSILON)\n",
    "\n",
    "                optimize_model(agent, target_agent, memory, optimizer, BATCH_SIZE, GAMMA)\n",
    "                if env.lane_invasion:\n",
    "                    done = True\n",
    "                \n",
    "                if done:\n",
    "                    break\n",
    "\n",
    "            # Update target network\n",
    "            if episode % 10 == 0:\n",
    "                target_agent.load_state_dict(agent.state_dict())\n",
    "\n",
    "            print(f\"Episode {episode}, Total Reward: {episode_reward}, Epsilon: {epsilon}\")\n",
    "            scores.append(episode_reward)\n",
    "            time.sleep(0.01)\n",
    "        \n",
    "            \n",
    "        # Save the trained model\n",
    "        torch.save(agent.state_dict(), \"dqn_agent.pth\")\n",
    "    finally:\n",
    "        # Destroy the vehicle in CARLA\n",
    "        sensor.destroy()                      \n",
    "        vehicle.destroy()\n",
    "        print(\"All Cleared\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 104,
   "id": "7e2a3bd8",
   "metadata": {},
   "outputs": [],
   "source": [
    "if __name__ == \"__main__\":\n",
    "    main()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5962af22",
   "metadata": {},
   "outputs": [],
   "source": [
    "[(-23.597307205200195, 137.0661163330078, 0.3521270751953125), (-23.55245018005371, 137.06639099121094, 0.3521270751953125), (-22.7453670501709, 137.07135009765625, 0.3521270751953125), (-20.906631469726562, 137.0826416015625, 0.3521270751953125), (-17.937257766723633, 137.10089111328125, 0.3521270751953125), (-14.001919746398926, 137.1250762939453, 0.3521270751953125), (-9.950039863586426, 137.1499786376953, 0.3521270751953125), (-5.889456748962402, 137.17494201660156, 0.3521270751953125), (-1.899501919746399, 137.199462890625, 0.3521270751953125), (2.1661477088928223, 137.22511291503906, 0.4448866546154022), (6.19586181640625, 137.272705078125, 0.9083132743835449), (10.269187927246094, 137.3509063720703, 1.1775866746902466), (14.2843017578125, 137.41847229003906, 0.7351945042610168), (18.339458465576172, 137.4542999267578, 0.32044845819473267), (22.50907325744629, 137.47763061523438, 0.32044845819473267), (26.559429168701172, 137.50027465820312, 0.32044845819473267), (30.52979850769043, 137.52249145507812, 0.32044845819473267), (34.64375305175781, 137.54550170898438, 0.32044845819473267), (38.67440414428711, 137.5680389404297, 0.32044845819473267), (42.72395324707031, 137.59068298339844, 0.32044845819473267), (46.77073287963867, 137.6133270263672, 0.32044845819473267), (50.81194305419922, 137.63592529296875, 0.32044845819473267), (54.853050231933594, 137.6585235595703, 0.32044845819473267), (58.88810729980469, 137.68109130859375, 0.32044845819473267), (62.942222595214844, 137.70376586914062, 0.32044845819473267), (67.03063201904297, 137.65335083007812, -3.2371885776519775), (70.9086685180664, 137.23367309570312, -9.11582088470459), (75.0324478149414, 136.33480834960938, -15.477066040039062), (78.87277221679688, 135.04766845703125, -21.581363677978516), (82.56835174560547, 133.35215759277344, -27.709299087524414), (86.10346221923828, 131.243896484375, -33.912811279296875), (89.42973327636719, 128.73248291015625, -40.19452667236328), (92.34596252441406, 125.9930419921875, -46.22463607788086), (94.98958587646484, 122.92913818359375, -51.8900260925293), (97.37153625488281, 119.6259536743164, -56.51860427856445), (99.46526336669922, 116.16654968261719, -61.11445236206055), (101.27847290039062, 112.54296112060547, -65.71971130371094), (102.79850006103516, 108.77535247802734, -70.33717346191406), (104.00875854492188, 104.9033432006836, -74.94791412353516), (104.90636444091797, 100.9328842163086, -79.574462890625), (105.48133087158203, 96.897705078125, -84.20702362060547), (105.7305908203125, 92.72013092041016, -88.96361541748047), (105.76097869873047, 88.73402404785156, -89.6092529296875), (105.78852081298828, 84.69615173339844, -89.6092529296875), (105.8160629272461, 80.65655517578125, -449.6092224121094), (105.84347534179688, 76.637939453125, -449.6092224121094), (105.87109375, 72.58792114257812, -449.6092224121094), (105.89848327636719, 68.57239532470703, -449.6092224121094), (105.92497253417969, 64.6881332397461, -449.6092224121094), (105.94950866699219, 61.08949661254883, -449.6092224121094), (105.9720458984375, 57.78582763671875, -449.6092224121094), (105.99339294433594, 54.65508270263672, -449.6092224121094), (106.01345825195312, 51.713321685791016, -89.6092529296875), (106.03005981445312, 49.278953552246094, -89.6092529296875), (106.03091430664062, 49.153141021728516, -89.6092529296875), (106.03091430664062, 49.15312957763672, -89.6092529296875), (106.03091430664062, 49.1533203125, -89.6092529296875), (106.03091430664062, 49.153316497802734, -89.6092529296875), (106.03091430664062, 49.153316497802734, -89.6092529296875), (106.03091430664062, 49.153316497802734, -89.6092529296875)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "id": "17fc5964",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "45.0"
      ]
     },
     "execution_count": 35,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "math.atan2(1,1)*180/math.pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 105,
   "id": "f3a1ac05",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "plt.plot(scores)\n",
    "plt.title(\"Training Curve\")\n",
    "plt.xlabel('No of Episodes')\n",
    "plt.ylabel(\"Cummulative scrore per Episode\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2722918",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}