CARLA_Town06_dev.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import carla\n",
    "from carla import Actor\n",
    "from carla import Vector3D\n",
    "from carla import Transform, Location, Rotation\n",
    "\n",
    "from datetime import date\n",
    "from datetime import datetime\n",
    "\n",
    "# track CPU & GPU usage\n",
    "import psutil\n",
    "import subprocess\n",
    "import sys\n",
    "import time\n",
    "\n",
    "\n",
    "###\n",
    "# CONSTANT VARIABLES\n",
    "###\n",
    "RELATIVE_PATH = \"./output_data/\"\n",
    "ABSOLUTE_PATH = \"/home/carla/CarlaTable/tables/\"\n",
    "DEBUG_LOG_PATH = \"/home/carla/CarlaTable/log/debug_log_\"\n",
    "DELTA_V_CAP = 1e-5\n",
    "FREQUENCY = 100 # HZ\n",
    "MIN_SECONDS_EACH_RUN = 2\n",
    "MAX_SECONDS_EACH_RUN = 5\n",
    "DELTA_T = 1 / FREQUENCY\n",
    "HEADER = [\"velocity_x(m/s), velocity_y(m/s), velocity_z(m/s)\", \"location_x, location_y, location_z\", \\\n",
    "         \"pitch, yaw, roll\", \"acceleration_x(m/s^2), acceleration_y(m/s^2), acceleration_z(m/s^2)\", \\\n",
    "         \"angular_velocity_x(deg/s), angular_velocity_y(deg/s), angular_velocity_z(deg/s)\"]\n",
    "\n",
    "\n",
    "# Connect to the client and retrieve the world object\n",
    "client = carla.Client('localhost', 2000)\n",
    "# load map \n",
    "# NOTE: It is critical to load_world FIRST before\n",
    "# calling \"world = cliend.get_world()\"\n",
    "client.load_world('Town06')\n",
    "world = client.get_world()\n",
    "settings = world.get_settings()\n",
    "\n",
    "# set rendering mode\n",
    "# settings.no_rendering_mode = True\n",
    "\n",
    "settings.synchronous_mode = True # Enables synchronous mode\n",
    "settings.fixed_delta_seconds = DELTA_T\n",
    "world.apply_settings(settings)\n",
    "\n",
    "# get blueprintthrottle = 0.4\n",
    "level = world.get_map()\n",
    "weather = world.get_weather()\n",
    "blueprint_library = world.get_blueprint_library()\n",
    "elapsed_seconds_since_world_starts = world.get_snapshot().elapsed_seconds\n",
    "# Get the blueprint library and filter for the vehicle blueprints\n",
    "# vehicle_blueprints = world.get_blueprint_library().filter('*vehicle*')\n",
    "\n",
    "\n",
    "\n",
    "def poll_gpus(flatten=False):\n",
    "    \"\"\"\n",
    "    Query GPU utilisation, and sanitise results\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    list of lists of utilisation stats\n",
    "        For each GPU (outer list), there is a list of utilisations\n",
    "        corresponding to each query (inner list), as a string.\n",
    "    \"\"\"\n",
    "    res = subprocess.check_output(\n",
    "        ['nvidia-smi',\n",
    "         '--query-gpu=' + 'utilization.gpu,utilization.memory,temperature.gpu',\n",
    "         '--format=csv,nounits,noheader']\n",
    "        )\n",
    "    lines = [i_res for i_res in res.decode().split('\\n') if i_res != '']\n",
    "    data = [[val.strip() if 'Not Supported' not in val else 'N/A'\n",
    "             for val in line.split(',')\n",
    "             ] for line in lines]\n",
    "    if flatten:\n",
    "        data = [y for row in data for y in row]\n",
    "    return data\n",
    "\n",
    "def poll_cpu():\n",
    "    \"\"\"\n",
    "    Fetch current CPU, RAM and Swap utilisation\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    float\n",
    "        CPU utilisation (percentage)\n",
    "    float\n",
    "        RAM utilisation (percentage)\n",
    "    float\n",
    "        Swap utilisation (percentage)\n",
    "    \"\"\"\n",
    "    return (\n",
    "        psutil.cpu_percent(),\n",
    "        psutil.virtual_memory().percent,\n",
    "        psutil.swap_memory().percent,\n",
    "        )\n",
    "\n",
    "\n",
    "\n",
    "# good spawn point for Town06\n",
    "# ego_vehicle_spawn_point = Transform(Location(x=-272, y=-18, z=0.281494), \\\n",
    "#                                     Rotation(pitch=0.000000, yaw=0.0, roll=0.000000))\n",
    "# ego_vehicle = world.spawn_actor(blueprint_library.find('vehicle.tesla.model3'), \\\n",
    "#                                 ego_vehicle_spawn_point)\n",
    "# ego_vehicle = world.spawn_actor(blueprint_library.find('vehicle.ford.mustang'), \\\n",
    "#                                 ego_vehicle_spawn_point)\n",
    "\n",
    "\n",
    "# sets the camera to focus on ego_vehicle\n",
    "def setSpectator(ego_vehicle: Actor, spectator: Actor) -> None:\n",
    "    spectator_transform = ego_vehicle.get_transform()\n",
    "    spectator.set_transform(carla.Transform(spectator_transform.location + \\\n",
    "                            carla.Location(x=0, y=0,z=30), carla.Rotation(pitch=-90)))\n",
    "\n",
    "\n",
    "# helper function, collects data to be recorded\n",
    "def get_vehicle_velocity_info(vehicle: Actor) -> str:\n",
    "    vehicle_velocity = vehicle.get_velocity()\n",
    "    data = str(vehicle_velocity.x) + \",\" \\\n",
    "         + str(vehicle_velocity.y) + \",\" \\\n",
    "         + str(vehicle_velocity.z)\n",
    "    return data\n",
    "\n",
    "\n",
    "def get_vehicle_location_info(vehicle: Actor) -> str:\n",
    "    vehicle_transform = vehicle.get_transform()\n",
    "    vehicle_location = vehicle_transform.location\n",
    "    data = str(vehicle_location.x) + \",\" \\\n",
    "         + str(vehicle_location.y) + \",\" \\\n",
    "         + str(vehicle_location.z)\n",
    "    return data\n",
    "    \n",
    "    \n",
    "def get_vehicle_rotation_info(vehicle: Actor) -> str:\n",
    "    vehicle_transform = vehicle.get_transform()\n",
    "    vehicle_rotation = vehicle_transform.rotation  # pitch-y, yaw-z, roll-x\n",
    "    data = str(vehicle_rotation.pitch) + \",\" \\\n",
    "         + str(vehicle_rotation.yaw) + \",\" \\\n",
    "         + str(vehicle_rotation.roll)\n",
    "    return data\n",
    "\n",
    "\n",
    "def get_vehicle_acceleration_info(vehicle: Actor) -> str:\n",
    "    vehicle_acceleration = vehicle.get_acceleration()\n",
    "    data = str(vehicle_acceleration.x) + \",\" \\\n",
    "         + str(vehicle_acceleration.y) + \",\" \\\n",
    "         + str(vehicle_acceleration.z)\n",
    "    return data\n",
    "\n",
    "\n",
    "def get_angular_velocity_info(vehicle:Actor) -> str:\n",
    "    vehicle_angular_velocity = vehicle.get_angular_velocity()\n",
    "    data = str(vehicle_angular_velocity.x) + \",\" \\\n",
    "         + str(vehicle_angular_velocity.y) + \",\" \\\n",
    "         + str(vehicle_angular_velocity.z)\\\n",
    "#     if(vehicle_angular_velocity == None):\n",
    "#         print(\"angular_velocity_not_available\")\n",
    "    return data\n",
    "\n",
    "\n",
    "# return name of the file that raw data will write to  \n",
    "def create_file_name(ego_vehicle_throttle: float, ego_vehicle_steer: float) -> str:\n",
    "    today = date.today()\n",
    "    now = datetime.now()\n",
    "    current_time = now.strftime(\"%H:%M:%S\")\n",
    "    throttle_name = str(throttle)\n",
    "    while(len(throttle_name) < 5): # 0.100\n",
    "        throttle_name += '0'\n",
    "    steer_name = str(steer)\n",
    "    while(len(steer_name) < 4): # 0.10\n",
    "        steer_name += '0'\n",
    "    file_name = \"raw_data_throttle=\" + throttle_name + \"_steer=\" \\\n",
    "              + steer_name + \"_\" + str(today) + \"_\" + str(current_time) +\".csv\"\n",
    "    return file_name\n",
    "\n",
    "\n",
    "def write_to_file(file_name: str, data: str) -> None:\n",
    "    f = open(file_name, \"a\")\n",
    "    f.write(data)\n",
    "    f.close()\n",
    "    return None\n",
    "\n",
    "### Constants\n",
    "REC_FUNC = [get_vehicle_velocity_info, get_vehicle_location_info, get_vehicle_rotation_info, \\\n",
    "           get_vehicle_acceleration_info, get_angular_velocity_info]\n",
    "\n",
    "\n",
    "### functions for exit conditions for the main while loop\n",
    "# def in_time_period(elapsed_seconds: float, duration: float) -> bool:\n",
    "#     return\n",
    "\n",
    "# desired road: Town06\n",
    "# max location: carla.Location(x=-272, y=-21.5, z=0.281494)\n",
    "# max location: carla.Location(x=-272, y=-10.5, z=0.281494)\n",
    "# max locatoin: carla.Location(x=-625, y=-21.5, z=0.281494)\n",
    "# max location: carla.Location(x=-625, y=-10.5, z=0.281494)\n",
    "def vehicle_is_in_road(vehicle: Actor) -> bool:\n",
    "#     print(\"x:\"+str((vehicle.get_location().y > -21.6) & (vehicle.get_location().y < -10.5)))\n",
    "#     print(vehicle.get_location().y)\n",
    "    return ((vehicle.get_location().x > -272.0) & (vehicle.get_location().x < 625.0))\n",
    "#     return ((vehicle.get_location().x > -272.0) & (vehicle.get_location().x < 625.0) & \\\n",
    "#             (vehicle.get_location().y > -21.5) & (vehicle.get_location().y < -10.5) & \\\n",
    "#             (vehicle.get_location().z > 0.0) & (vehicle.get_location().z < 0.5)) \n",
    "\n",
    "\n",
    "# def vehicle_is_in_road(vehicle: Actor, world: carla.World) -> bool:\n",
    "#     if(world.get_map().name[-6:].__eq__(\"Town06\")):\n",
    "#         print(str(vehicle.get_location().z))\n",
    "        \n",
    "#         return ((vehicle.get_location().x >= -272.0) & (vehicle.get_location().x <= 625.0) & \\\n",
    "#                 (vehicle.get_location().y >= -21.5) & (vehicle.get_location().y <= -10.5) & \\\n",
    "#                 (vehicle.get_location().z >= 0.0) & (vehicle.get_location().z <= 0.5))\n",
    "#     return False\n",
    "\n",
    "\n",
    "# return True if ego vehicle is moving at constant velocity for 10 consecutive ticks\n",
    "vehicle_at_const_speed_counter = 0\n",
    "def vehicle_at_const_speed(ego_vehicle_velocity: Vector3D, pre_ego_vehicle_velocity: Vector3D, \\\n",
    "                          vehicle_at_const_speed_counter: int) -> bool:\n",
    "    rtn = ((abs(ego_vehicle_velocity.x - pre_ego_vehicle_velocity.x) < DELTA_V_CAP) & \\\n",
    "           (abs(ego_vehicle_velocity.y - pre_ego_vehicle_velocity.y) < DELTA_V_CAP) & \\\n",
    "           (abs(ego_vehicle_velocity.z - pre_ego_vehicle_velocity.z) < DELTA_V_CAP))\n",
    "    if(rtn):\n",
    "        vehicle_at_const_speed_counter += 1\n",
    "    else:\n",
    "        vehicle_at_const_speed_counter = 0\n",
    "    if(vehicle_at_const_speed_counter >= 10):\n",
    "        vehicle_at_const_speed_counter = 0\n",
    "        return True\n",
    "    return False\n",
    "\n",
    "# \"\\N{DEGREE SIGN}\"\n",
    "def log_debug_info(file_name: str, frame: int, elapsed_seconds: float) -> None:\n",
    "    gpu_data = str(poll_gpus())[2:]\n",
    "    cpu_data = str(poll_cpu()[0]) + \",\" + str(poll_cpu()[1]) + \",\" + str(poll_cpu()[2])\n",
    "    data = gpu_data + cpu_data + \"\\n\"\n",
    "    write_to_file(file_name, data)\n",
    "        \n",
    "        \n",
    "    \n",
    "# main loop where all key actions take place\n",
    "def loop(throttle: float, steer: float, ego_vehicle: Actor, spectator: Actor, \\\n",
    "         file_name: str, rec_choice: list) -> None:\n",
    "    # initialization\n",
    "    start_time_in_real_world = datetime.utcnow()\n",
    "    pre_ego_vehicle_velocity = Vector3D(0.0, 0.0, 0.0)\n",
    "    frame_zero = world.get_snapshot().frame\n",
    "    at_const_speed = False\n",
    "    elapsed_seconds = 0.0\n",
    "    simulation_time_stamp_this_run = world.get_snapshot().elapsed_seconds\n",
    "    vehicle_at_const_speed_counter = 0\n",
    "#     while True:\n",
    "#     while elapsed_seconds <= 100:\n",
    "#     while (not at_const_speed):\n",
    "#     print(vehicle_is_in_road(ego_vehicle, elapsed_seconds))\n",
    "    while (elapsed_seconds <= MIN_SECONDS_EACH_RUN) | \\\n",
    "          ( (elapsed_seconds <= MAX_SECONDS_EACH_RUN) & vehicle_is_in_road(ego_vehicle)):\n",
    "#            & (not at_const_speed) & vehicle_is_in_road(ego_vehicle) ):\n",
    "                                    \n",
    "        setSpectator(ego_vehicle, spectator)\n",
    "        \n",
    "        ego_vehicle_throttle = throttle\n",
    "        ego_vehicle_steer = steer\n",
    "        ego_vehicle.apply_control(carla.VehicleControl(\\\n",
    "                        throttle=ego_vehicle_throttle, steer=ego_vehicle_steer))\n",
    "        \n",
    "        snapshot = world.get_snapshot()\n",
    "        frame = snapshot.frame - frame_zero\n",
    "        elapsed_seconds = snapshot.elapsed_seconds - simulation_time_stamp_this_run\n",
    "        delta_seconds = snapshot.delta_seconds\n",
    "        platform_timestamp = snapshot.platform_timestamp\n",
    "        \n",
    "#         print(elapsed_seconds)\n",
    "        ego_vehicle_velocity = ego_vehicle.get_velocity()\n",
    "        at_const_speed = vehicle_at_const_speed(ego_vehicle_velocity, pre_ego_vehicle_velocity, elapsed_seconds)\n",
    "#         print(at_const_speed)\n",
    "        delta_velocity = Vector3D(ego_vehicle_velocity.x - pre_ego_vehicle_velocity.x, \n",
    "                                  ego_vehicle_velocity.y - pre_ego_vehicle_velocity.y,\n",
    "                                  ego_vehicle_velocity.z - pre_ego_vehicle_velocity.z)\n",
    "        pre_ego_vehicle_velocity = ego_vehicle_velocity\n",
    "        \n",
    "        # write debu0g log\n",
    "        \n",
    "        log_debug_info(DEBUG_LOG_PATH + file_name, frame, elapsed_seconds)\n",
    "#         write_to_file(DEBUG_LOG_PATH + file_name, str(delta_velocity)+\"\\n\")\n",
    "        # write to table\n",
    "        data = str(frame) + \",\" + str((datetime.utcnow() - start_time_in_real_world).total_seconds()) \\\n",
    "             + \",\" + str(elapsed_seconds) + \",\" + str(delta_seconds) + \",\" + str(platform_timestamp)\n",
    "        for i in range(5):\n",
    "            if rec_choice[i]:\n",
    "                data += \", \" + REC_FUNC[i](ego_vehicle)\n",
    "        data += \"\\n\"\n",
    "        write_to_file(ABSOLUTE_PATH + file_name, data)\n",
    "        world.tick()\n",
    "    #end of while\n",
    "#     ego_vehicle.\n",
    "    return None\n",
    "\n",
    "\n",
    "# wraper of the main loop and handles trivials\n",
    "def run_once(ego_vehicle: Actor, throttle: float, steer: float, \\\n",
    "            rec_choice = [{\"rec_velocity\":True}, {\"rec_location\":True}, \\\n",
    "                          {\"rec_rotation\":True}, {\"rec_acceleration\":True}, \\\n",
    "                          {\"rec_angular_velocity\":True}]) -> None:\n",
    "#     ego_vehicle.show_debug_telemetry(True)\n",
    "    data = \"frame, real_world_time_stamp, elapsed_seconds, delta_seconds, platform_timestamp\"\n",
    "    for i in range(5):\n",
    "        if rec_choice[i]:\n",
    "            data += \", \"\n",
    "            data += HEADER[i]\n",
    "    data += \"\\n\"\n",
    "    spectator = world.get_spectator()\n",
    "    file_name = create_file_name(throttle, steer)\n",
    "    debug_header = \"frame number, elapsed_seconds, GPU Usage %, GPU Mem Usage, \\\n",
    "              GPU Temperature, CPU Usage, System RAM, Swap, delta_velocity\\n\"\n",
    "    write_to_file(DEBUG_LOG_PATH + file_name, debug_header)\n",
    "    write_to_file(ABSOLUTE_PATH + file_name, data)\n",
    "    loop(throttle, steer, ego_vehicle, spectator, file_name, rec_choice)\n",
    "    return None\n",
    "\n",
    "# choice of data needed\n",
    "# 5 parameters corresponds to \n",
    "# velocity, location, rotation, acceleration, angular_velocity\n",
    "rec_choice = [1, 1, 1, 1, 1]\n",
    "throttle_delta = 0.001\n",
    "throttle = 0.0\n",
    "steer = 0.0\n",
    "# for run in range(397, 501):\n",
    "#     ego_vehicle_spawn_point = Transform(Location(x=-272, y=-18, z=0.281494), \\\n",
    "#                                 Rotation(pitch=0.000000, yaw=0.0, roll=0.000000))\n",
    "#     ego_vehicle = world.spawn_actor(blueprint_library.find('vehicle.tesla.model3'), \\\n",
    "#                                 ego_vehicle_spawn_point)\n",
    "#     throttle = round(throttle_delta * run, 3)\n",
    "#     run_once(ego_vehicle, throttle, steer, rec_choice)\n",
    "#     destroyed = ego_vehicle.destroy()\n",
    "\n",
    "\n",
    "###\n",
    "# TO-DO: ADD REAL-WORLD-TIME-STAMP IN LOG AND DEBUG INFO\n",
    "###\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "ego_vehicle_spawn_point = Transform(Location(x=-272, y=-18, z=0.281494), \\\n",
    "                                Rotation(pitch=0.000000, yaw=0.0, roll=0.000000))\n",
    "ego_vehicle = world.spawn_actor(blueprint_library.find('vehicle.tesla.model3'), \\\n",
    "                                ego_vehicle_spawn_point)\n",
    "throttle = 0.4\n",
    "run_once(ego_vehicle, throttle, steer, rec_choice)\n",
    "destroyed = ego_vehicle.destroy()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# https://softhints.com/increase-cell-width-jupyter-notebook/\n",
    "\n",
    "# from IPython.display import display, HTML\n",
    "# display(HTML(\"<style>.container { width:75% !important; }</style>\"))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import psutil\n",
    "# import subprocess\n",
    "# import sys\n",
    "# import time\n",
    "\n",
    "#     def poll_gpus(self, flatten=False):\n",
    "#         \"\"\"\n",
    "#         Query GPU utilisation, and sanitise results\n",
    "\n",
    "#         Returns\n",
    "#         -------\n",
    "#         list of lists of utilisation stats\n",
    "#             For each GPU (outer list), there is a list of utilisations\n",
    "#             corresponding to each query (inner list), as a string.\n",
    "#         \"\"\"\n",
    "#         res = subprocess.check_output(\n",
    "#             ['nvidia-smi',\n",
    "#              '--query-gpu=' + self.gpu_query,\n",
    "#              '--format=csv,nounits,noheader']\n",
    "#             )\n",
    "#         lines = [i_res for i_res in res.decode().split('\\n') if i_res != '']\n",
    "#         data = [[val.strip() if 'Not Supported' not in val else 'N/A'\n",
    "#                  for val in line.split(',')\n",
    "#                  ] for line in lines]\n",
    "#         if flatten:\n",
    "#             data = [y for row in data for y in row]\n",
    "#         return data\n",
    "\n",
    "#     def poll_cpu(self):\n",
    "#         \"\"\"\n",
    "#         Fetch current CPU, RAM and Swap utilisation\n",
    "\n",
    "#         Returns\n",
    "#         -------\n",
    "#         float\n",
    "#             CPU utilisation (percentage)\n",
    "#         float\n",
    "#             RAM utilisation (percentage)\n",
    "#         float\n",
    "#             Swap utilisation (percentage)\n",
    "#         \"\"\"\n",
    "#         return (\n",
    "#             psutil.cpu_percent(),\n",
    "#             psutil.virtual_memory().percent,\n",
    "#             psutil.swap_memory().percent,\n",
    "#             )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "carla914",
   "language": "python",
   "name": "carla914"
  },
  "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.7.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}