tutorials/nuplan_planner_tutorial.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "63ae56b0",
   "metadata": {},
   "source": [
    "![](https://www.nuplan.org/static/media/nuPlan_final.3fde7586.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7e01e5d2",
   "metadata": {},
   "source": [
    "### Contents\n",
    "\n",
    "1. [Introduction](#introduction)\n",
    "2. [Creating the planner](#planning)\n",
    "3. [Simulating the planner](#simulation)\n",
    "4. [Visualizing metrics and scenarios](#dashboard)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "057e7cea",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tutorials.utils.tutorial_utils import setup_notebook\n",
    "\n",
    "setup_notebook()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "34460db1",
   "metadata": {},
   "source": [
    "# Creating a new planner in nuPlan <a name=\"introduction\"></a>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "01b6b6ee",
   "metadata": {},
   "source": [
    "Welcome to the nuplan planning tutorial. One of the core interactions with nuplan will be to build and train your own planners and run them through nuplan's simulation pipeline for evaluation. While we discuss how to train your machine learned planners in other tutorials, in this tutorial we will show you the basic elements you will need for constructing a planner that plugs into the nuplan interface. We will also show you the basics of running the planner in simulation and visualizing the behavior and metrics for evaluation."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f0189156",
   "metadata": {},
   "source": [
    "## Setup\n",
    "\n",
    "To be able to access all resources within this notebook, make sure Jupyter is launched at the root of this repo. The path of the notebook should be `/notebook/<repo_root>`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13c00121",
   "metadata": {},
   "outputs": [],
   "source": [
    "# (Optional) Increase notebook width for all embedded cells to display properly\n",
    "from IPython.core.display import display, HTML\n",
    "\n",
    "display(HTML(\"<style>.output_result { max-width:100% !important; }</style>\"))\n",
    "display(HTML(\"<style>.container { width:100% !important; }</style>\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "779b0132",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Useful imports\n",
    "import os\n",
    "from pathlib import Path\n",
    "import tempfile\n",
    "\n",
    "import hydra"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "edf9156f",
   "metadata": {},
   "source": [
    "# Creating the planner <a name=\"planning\"></a>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b21f4f69",
   "metadata": {},
   "source": [
    "The planner is the main component responsible for determining the ego vehicle's behavior. At it's core, a planner will consume state information in the form of observations regarding its own pose, poses of other agents, and various static and dynamic map information, along with a goal, and produce a trajectory dictating the future path that the ego vehicle will attempt to follow according to a given control strategy.\n",
    "\n",
    "<img src=\"media/planner_inputs_outputs_diagram.drawio.svg\"/>\n",
    "\n",
    "In nuplan, all planners will inherit from the *AbstractPlanner* class and, along with whatever additional functionality you provide it, will have to implement a few core methods. Let's take a look at the *AbstractPlanner* class."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "42ff5db7",
   "metadata": {},
   "source": [
    "Let's take a look at these core methods one by one.\n",
    "\n",
    "**initialize**: This method initializes the planner with important static information including the high level goal, represented as a (x, y, heading) pose, which in practice might be provided a higher level routing system, as well as the interface for interacting with relevant map information.\n",
    "\n",
    "**name**: The name of the planner. This one's easy!\n",
    "\n",
    "**observation_type**: This dictates what type of observations the planner will consume to inform it's decision making. Options here include *Sensors* (raw sensor information such as images or pointclouds) and *DetectionsTracks* (outputs of an earlier perception system designed to consume sensor information and produce meaningful detections).\n",
    "\n",
    "**compute_trajectory**: This is the core behavior of the planner, and the part you will most likely spend the most time on. This method will consume a history buffer containing discretized past ego trajectory information as well as observations of the type declared in *observation_type* up until the current time step. Correctly accounting for historical as well as current information will allow the planner to produce more stable and strategic behaviors and avoid overcorrecting in the presence of noisy observations. The *compute_trajectory* method will be responsible for producing the trajectory dictating the path the ego vehicle will attempt to follow in the future. The trajectory represents the series of ego states (identifying pose along with dynamic information such as velocities and accelerations) the vehicle will try to attain along a specified time horizon, most easily represented as a list of states along with a method to interpolate between them.\n",
    "\n",
    "\n",
    "We will now use the *SimplePlanner* class as an example of how to implement the *AbstractPlanner* interface with a real planner."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d8f1b203",
   "metadata": {},
   "outputs": [],
   "source": [
    "from typing import List, Type\n",
    "\n",
    "import numpy as np\n",
    "import numpy.typing as npt\n",
    "\n",
    "from nuplan.common.actor_state.ego_state import DynamicCarState, EgoState\n",
    "from nuplan.common.actor_state.state_representation import StateSE2, StateVector2D, TimePoint\n",
    "from nuplan.common.actor_state.vehicle_parameters import get_pacifica_parameters, VehicleParameters\n",
    "from nuplan.planning.simulation.observation.observation_type import DetectionsTracks, Observation\n",
    "from nuplan.planning.simulation.planner.abstract_planner import AbstractPlanner, PlannerInitialization, PlannerInput\n",
    "from nuplan.planning.simulation.trajectory.interpolated_trajectory import  InterpolatedTrajectory\n",
    "from nuplan.planning.simulation.trajectory.abstract_trajectory import AbstractTrajectory\n",
    "from nuplan.planning.simulation.controller.motion_model.kinematic_bicycle import KinematicBicycleModel\n",
    "\n",
    "\n",
    "class SimplePlanner(AbstractPlanner):\n",
    "    \"\"\"\n",
    "    Planner going straight\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self,\n",
    "                 horizon_seconds: float,\n",
    "                 sampling_time: float,\n",
    "                 acceleration: npt.NDArray[np.float32],\n",
    "                 max_velocity: float = 5.0,\n",
    "                 steering_angle: float = 0.0):\n",
    "        self.horizon_seconds = TimePoint(int(horizon_seconds * 1e6))\n",
    "        self.sampling_time = TimePoint(int(sampling_time * 1e6))\n",
    "        self.acceleration = StateVector2D(acceleration[0], acceleration[1])\n",
    "        self.max_velocity = max_velocity\n",
    "        self.steering_angle = steering_angle\n",
    "        self.vehicle = get_pacifica_parameters()\n",
    "        self.motion_model = KinematicBicycleModel(self.vehicle)\n",
    "\n",
    "    def initialize(self, initialization: List[PlannerInitialization]) -> None:\n",
    "        \"\"\" Inherited, see superclass. \"\"\"\n",
    "        pass\n",
    "\n",
    "    def name(self) -> str:\n",
    "        \"\"\" Inherited, see superclass. \"\"\"\n",
    "        return self.__class__.__name__\n",
    "\n",
    "    def observation_type(self) -> Type[Observation]:\n",
    "        \"\"\" Inherited, see superclass. \"\"\"\n",
    "        return DetectionsTracks  # type: ignore\n",
    "\n",
    "    def compute_planner_trajectory(self, current_input: PlannerInput) -> List[AbstractTrajectory]:\n",
    "        \"\"\"\n",
    "        Implement a trajectory that goes straight.\n",
    "        Inherited, see superclass.\n",
    "        \"\"\"\n",
    "        # Extract iteration and history\n",
    "        iteration = current_input.iteration\n",
    "        history = current_input.history\n",
    "\n",
    "        ego_state = history.ego_states[-1]\n",
    "        state = EgoState(\n",
    "            car_footprint=ego_state.car_footprint,\n",
    "            dynamic_car_state=DynamicCarState.build_from_rear_axle(\n",
    "                ego_state.car_footprint.rear_axle_to_center_dist,\n",
    "                ego_state.dynamic_car_state.rear_axle_velocity_2d,\n",
    "                self.acceleration,\n",
    "            ),\n",
    "            tire_steering_angle=self.steering_angle,\n",
    "            is_in_auto_mode=True,\n",
    "            time_point=ego_state.time_point,\n",
    "        )\n",
    "        trajectory: List[EgoState] = [state]\n",
    "        for _ in np.arange(\n",
    "            iteration.time_us + self.sampling_time.time_us,\n",
    "            iteration.time_us + self.horizon_seconds.time_us,\n",
    "            self.sampling_time.time_us,\n",
    "        ):\n",
    "            if state.dynamic_car_state.speed > self.max_velocity:\n",
    "                accel = self.max_velocity - state.dynamic_car_state.speed\n",
    "                state = EgoState.build_from_rear_axle(\n",
    "                    rear_axle_pose=state.rear_axle,\n",
    "                    rear_axle_velocity_2d=state.dynamic_car_state.rear_axle_velocity_2d,\n",
    "                    rear_axle_acceleration_2d=StateVector2D(accel, 0),\n",
    "                    tire_steering_angle=state.tire_steering_angle,\n",
    "                    time_point=state.time_point,\n",
    "                    vehicle_parameters=state.car_footprint.vehicle_parameters,\n",
    "                    is_in_auto_mode=True,\n",
    "                    angular_vel=state.dynamic_car_state.angular_velocity,\n",
    "                    angular_accel=state.dynamic_car_state.angular_acceleration,\n",
    "                )\n",
    "\n",
    "            state = self.motion_model.propagate_state(state, state.dynamic_car_state, self.sampling_time)\n",
    "            trajectory.append(state)\n",
    "\n",
    "        return InterpolatedTrajectory(trajectory)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1945b81c",
   "metadata": {},
   "source": [
    "The *SimplePlanner* is very simple indeed, electing to just drive straight according to a specified steering angle. It doesn't even take into account the observations it receives!\n",
    "\n",
    "The *initialize*, *name*, and *observation_type* methods are straightforward implementations of the *AbstractPlanner* interface. The observation type here is declared as *DetectionsTracks* for use with processed detections coming from the perception system, though we note that the observations are completely ignored in the *compute_trajectory* method (we would still want to change this to *Sensors* for use with raw sensor data such as lidar pointclouds). Furthermore, we note the additional *\\_\\_init\\_\\_* method that sets intrinsic parameters of the planner, such as what steering angle it will follow and by how much it will accelerate.\n",
    "\n",
    "The *compute_trajectory* method applies the motion model of the car to update the current velocity profile with the straight line acceleration specified at initialization, rolling this out according to the horizon also specified at initialization. A more advanced planner would take the observation history and map into account and try to reach the goal specified at initialization."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "128b0d15",
   "metadata": {},
   "source": [
    "# Simulating the planner <a name=\"simulation\"></a>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "10f14dbe",
   "metadata": {},
   "source": [
    "## Open-loop simulation\n",
    "Open-loop simulation aims to evaluate the policy's capabilities to imitate the expert driver's behavior.<br />\n",
    "This is essentially done through log replay as the policy's predictions do not affect the state of the simulation.\n",
    "\n",
    "As the policy is not in full control of the vehicle, this type of simulation can only provide a high-level performance overview.\n",
    "\n",
    "## Closed-loop simulation\n",
    "Conversely, in closed-loop simulation the policy's actions alter the state of the simulation which tries to closely approximate the real-world system.\n",
    "\n",
    "The simulation's feedback loop enables a more in-depth evaluation of the policy as compounding errors can cause future observations to significantly diverge from the ground truth.<br />\n",
    "This is important in measuring distribution shifts introduced due to lack of variance in training examples through pure imitation learning.\n",
    "\n",
    "Closed-loop simulation is further divided into two categories:\n",
    "* ego closed-loop simulation with agents replayed from log (open-loop, non reactive)\n",
    "* ego closed-loop simulation with agents controlled by a rule-based or learned policy (closed-loop, reactive)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f5770200",
   "metadata": {},
   "source": [
    "## Simulation parameters"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "22682633",
   "metadata": {},
   "source": [
    "### Ego Controller\n",
    "\n",
    "Change the controller the ego vehicle uses to track the trajectory supplied by the planner (in the case of closed-loop) or whether log replay is used (open-loop) with `ego_controller=X` where `X` is a config yaml defined in the table below. \n",
    "\n",
    "| Ego controller | Description | Config |\n",
    "| --- | --- | --- |\n",
    "| Log play back controller | Open-loop simulation via log replay | `log_play_back_controller` |\n",
    "| Perfect tracking controller | Ego perfectly tracks given trajectory (closed-loop) | `perfect_tracking_controller` |"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "55a51f0c",
   "metadata": {},
   "source": [
    "### Observation\n",
    "\n",
    "Change the type of observation supplied to the planner with `observation=X` where `X` is a config yaml defined in the table below. \n",
    "\n",
    "| Observation | Description | Config |\n",
    "| --- | --- | --- |\n",
    "| Box observation | Bounding boxes | `box_observation` |\n",
    "| IDM agents observation | Smart agent detections | `idm_agents_observation` | \n",
    "|Lidar pc observation | Lidar point clouds from the scenario | `lidar_pc_observation` |"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "db337ceb",
   "metadata": {},
   "source": [
    "## Prepare the simulation config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "11b08c6d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tutorials.utils.tutorial_utils import construct_simulation_hydra_paths\n",
    "\n",
    "# Location of paths with all simulation configs\n",
    "BASE_CONFIG_PATH = os.path.join(os.getenv('NUPLAN_TUTORIAL_PATH', ''), '../nuplan/planning/script')\n",
    "simulation_hydra_paths = construct_simulation_hydra_paths(BASE_CONFIG_PATH)\n",
    "\n",
    "# Create a temporary directory to store the simulation artifacts\n",
    "SAVE_DIR = tempfile.mkdtemp()\n",
    "\n",
    "# Select simulation parameters\n",
    "EGO_CONTROLLER = 'perfect_tracking_controller'  # [log_play_back_controller, perfect_tracking_controller]\n",
    "OBSERVATION = 'box_observation'  # [box_observation, idm_agents_observation, lidar_pc_observation]\n",
    "DATASET_PARAMS = [\n",
    "    'scenario_builder=nuplan_mini',  # use nuplan mini database (2.5h of 8 autolabeled logs in Las Vegas)\n",
    "    'scenario_filter=one_continuous_log',  # simulate only one log\n",
    "    \"scenario_filter.log_names=['2021.07.16.20.45.29_veh-35_01095_01486']\",\n",
    "    'scenario_filter.limit_total_scenarios=2',  # use 2 total scenarios\n",
    "]\n",
    "\n",
    "# Initialize configuration management system\n",
    "hydra.core.global_hydra.GlobalHydra.instance().clear()  # reinitialize hydra if already initialized\n",
    "hydra.initialize(config_path=simulation_hydra_paths.config_path)\n",
    "\n",
    "# Compose the configuration\n",
    "cfg = hydra.compose(config_name=simulation_hydra_paths.config_name, overrides=[\n",
    "    f'group={SAVE_DIR}',\n",
    "    f'experiment_name=planner_tutorial',\n",
    "    f'job_name=planner_tutorial',\n",
    "    'experiment=${experiment_name}/${job_name}',\n",
    "    'worker=sequential',\n",
    "    f'ego_controller={EGO_CONTROLLER}',\n",
    "    f'observation={OBSERVATION}',\n",
    "    f'hydra.searchpath=[{simulation_hydra_paths.common_dir}, {simulation_hydra_paths.experiment_dir}]',\n",
    "    'output_dir=${group}/${experiment}',\n",
    "    *DATASET_PARAMS,\n",
    "])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "88231b74",
   "metadata": {},
   "source": [
    "## Launch simulation (within the notebook)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "161cc166",
   "metadata": {},
   "outputs": [],
   "source": [
    "from nuplan.planning.script.run_simulation import run_simulation as main_simulation\n",
    "\n",
    "planner = SimplePlanner(horizon_seconds=10.0, sampling_time=0.25, acceleration=[0.0, 0.0])\n",
    "\n",
    "# Run the simulation loop (real-time visualization not yet supported, see next section for visualization)\n",
    "main_simulation(cfg, planner)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ba1cece",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get nuBoard simulation file for visualization later on\n",
    "simulation_file = [str(file) for file in Path(cfg.output_dir).iterdir() if file.is_file() and file.suffix == '.nuboard']"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c53b9c2e",
   "metadata": {},
   "source": [
    "# Visualizing metrics and scenarios  <a name=\"dashboard\"></a>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7514e91c",
   "metadata": {},
   "source": [
    "## nuBoard summary\n",
    "\n",
    "For visualization, we use nuBoard to evaluate the planner:\n",
    "* quantitatively, through common and scenario dependent metrics\n",
    "* qualitatively, through visualization of scenario progression\n",
    "\n",
    "### nuBoard tabs\n",
    "To achieve that, nuBoard has 3 core evaluation tabs:\n",
    "1. Overview - Scalar metrics summary of common and scenario metrics across the following categories:\n",
    "    * Ego dynamics\n",
    "    * Traffic violations\n",
    "    * Expert imitation\n",
    "    * Planning & navigation\n",
    "    * Scenario performance\n",
    "2. Histograms - Histograms over metric statistics for more a granular peek inside each metric focusing on:\n",
    "    * Metric statistics (e.g. min, max, p90)\n",
    "3. Scenarios - Low-level scenario visualizations:\n",
    "    * Time-series progression of a specific metric across a scenario\n",
    "    * Top-down visualization of the scenario across time for comparing predicted vs. expert trajectories\n",
    "\n",
    "In addition, there is a main configuration tab for selecting different simulation files for comparing planners/experiments.\n",
    "\n",
    "<br />\n",
    "\n",
    "**NOTE**: nuBoard is under heavy developement, overall functionality and aesthetics do not represent the final product!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c546ba9d",
   "metadata": {},
   "source": [
    "## Prepare the nuBoard config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24732d42",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tutorials.utils.tutorial_utils import construct_nuboard_hydra_paths\n",
    "\n",
    "# Location of paths with all nuBoard configs\n",
    "nuboard_hydra_paths = construct_nuboard_hydra_paths(BASE_CONFIG_PATH)\n",
    "\n",
    "# Initialize configuration management system\n",
    "hydra.core.global_hydra.GlobalHydra.instance().clear()  # reinitialize hydra if already initialized\n",
    "hydra.initialize(config_path=nuboard_hydra_paths.config_path)\n",
    "\n",
    "# Compose the configuration\n",
    "cfg = hydra.compose(config_name=nuboard_hydra_paths.config_name, overrides=[\n",
    "    'scenario_builder=nuplan_mini',  # set the database (same as simulation) used to fetch data for visualization\n",
    "    f'simulation_path={simulation_file}',  # nuboard file path, if left empty the user can open the file inside nuBoard\n",
    "    f'hydra.searchpath=[{nuboard_hydra_paths.common_dir}, {nuboard_hydra_paths.experiment_dir}]',\n",
    "])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "67acc351",
   "metadata": {},
   "source": [
    "## Launch nuBoard (open in new tab)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d1a59ee2",
   "metadata": {},
   "outputs": [],
   "source": [
    "from nuplan.planning.script.run_nuboard import main as main_nuboard\n",
    "\n",
    "# Run nuBoard\n",
    "main_nuboard(cfg)"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "5aea00fa936a67f14323fa2d54163d7dc1f328c617e433b03a680f73ee2dd426"
  },
  "kernelspec": {
   "display_name": "",
   "language": "python",
   "name": ""
  },
  "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.9.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}