{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial: Solving CommonRoad Planning Problems\n",
    "This tutorial demonstrates how we can use the (un)informed search algorithms, combined with motion primitives, to solve real planning problems introduced in CommonRoad [scenarios](https://commonroad.in.tum.de/scenarios). "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0. Preparation\n",
    "Before you proceed with this tutorial, make sure that\n",
    "\n",
    "* you have gone through the tutorials on **CommonRoad Input-Output** and **(Un)Informed Search Algorithms**.\n",
    "* you have installed all necessary modules for CommonRoad Search according to the installation manual.\n",
    "\n",
    "Let's start with importing relevant modules and classes for setting up the automaton and the CommonRoad (CR) scenario."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "\n",
    "# enabling autoreload will reload modules automatically before executing the code, so that\n",
    "# you can edit the code for your motion planners and execute them right away without reloading\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "\n",
    "import os\n",
    "import sys\n",
    "path_notebook = os.getcwd()\n",
    "\n",
    "# add the root folder to python path\n",
    "sys.path.append(os.path.join(path_notebook, \"../../\"))\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "from IPython import display\n",
    "\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "from commonroad.visualization.draw_dispatch_cr import draw_object\n",
    "\n",
    "from SMP.motion_planner.motion_planner import MotionPlanner, MotionPlannerType\n",
    "from SMP.maneuver_automaton.maneuver_automaton import ManeuverAutomaton\n",
    "from SMP.motion_planner.utility import plot_primitives"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we specify the motion planner that we want to use:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# uncomment the following lines to use different motion planners\n",
    "\n",
    "# type_motion_planner = MotionPlannerType.UCS\n",
    "# type_motion_planner = MotionPlannerType.GBFS\n",
    "type_motion_planner = MotionPlannerType.ASTAR\n",
    "# type_motion_planner = MotionPlannerType.STUDENT_EXAMPLE\n",
    "\n",
    "# your own motion planner can be called by uncommenting next line\n",
    "# type_motion_planner = MotionPlannerType.STUDENT"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Loading CR Scenario and Planning Problem Set\n",
    "In the next step, we load CommonRoad scenarios and their planning problems. Scenario ids beginning with prefix **\"C-\"** stand for cooeprative driving scenarios and possess multiple planning problems. The meaning of the symbols in a scenario are explained as follows:\n",
    "* **Dot**: initial state projected onto the position domain\n",
    "* **Red rectangle**: static obstacle\n",
    "* **Blue rectangle**: dynamic obstacle\n",
    "* **Yellow rectangle**: goal region projected onto the position domain"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# load scenario\n",
    "path_scenario = os.path.join(path_notebook, \"../../scenarios/exercise/\")\n",
    "id_scenario = 'USA_Lanker-1_2_T-1'\n",
    "\n",
    "# read in scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()\n",
    "# retrieve the first planning problem in the problem set\n",
    "planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# visualize scenario\n",
    "for i in range(0, 50):\n",
    "    display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(10, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    \n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Generating a Maneuver Automaton\n",
    "\n",
    "In the following, we load the pre-generated motion primitives from an XML-File, and generate a **Maneuver Automaton** out of them. The primitives used here \n",
    "* The motion primitives are generated for the *Kinematic Single Track Model* (see [Vehicle Model Documentation](https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf)) with parameters taken from vehicle model *BMW320i* (id_type_vehicle=2). \n",
    "* The motion primitives have a duration of 0.5 seconds, time step size of 0.1 seconds, and consists of 6 states. We assume constant input during this period. They have different velocities (ranging from 0 m/s to 20 m/s with step size of 4 m/s) and steering angles (ranging from -1.066 rad to 1.066 rad with a step size rounded to 0.18 rad) in their initial/final states, and resulting in a total of 167 distinct primitives.\n",
    "* Two motion primitives are considered connectable if the velocity and the steering angle of the final state of the preceding primitive are equal to those of the initial state of the following primitive.\n",
    "* By setting different ranges and steps for velocity and steering angle, one can obtain different sets of motion primitives: more primitives brings a higher coverage of state space when performing the search, but yields a higher branching factor and time of search; search with less primitives are usually faster, but chances are that no feasible trajectory leading to the goal region can be constructed with the set of primitives.\n",
    "\n",
    "We create two automata with 524 and 167 feasible motion primitives, respectively."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load the xml with stores the motion primitives\n",
    "name_file_motion_primitives = 'V_0.0_20.0_Vstep_2.0_SA_-1.066_1.066_SAstep_0.18_T_0.5_Model_BMW_320i.xml'\n",
    "# generate automaton\n",
    "automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)\n",
    "# plot motion primitives\n",
    "plot_primitives(automaton.list_primitives)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load the xml with stores the motion primitives\n",
    "name_file_motion_primitives = 'V_0.0_20.0_Vstep_4.0_SA_-1.066_1.066_SAstep_0.18_T_0.5_Model_BMW_320i.xml'\n",
    "# generate automaton\n",
    "automaton = ManeuverAutomaton.generate_automaton(name_file_motion_primitives)\n",
    "# plot motion primitives\n",
    "plot_primitives(automaton.list_primitives)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If necessary, you can also create your own set of motion primitives (refer to **tutorial_motion_primitive_generator.ipynb**)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Searching for solutions\n",
    "We now carry out the search for solutions with the automaton of 167 primitives.\n",
    "\n",
    "### 3.1 Initializing motion planner\n",
    "\n",
    "In this step, we would like to create a motion planner with the loaded scenario, planning problem, and the automaton."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# construct motion planner\n",
    "motion_planner = MotionPlanner.create(scenario=scenario, \n",
    "                                      planning_problem=planning_problem,\n",
    "                                      automaton=automaton, \n",
    "                                      motion_planner_type=type_motion_planner)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2 Executing the search\n",
    "Search for the solution can be executed by calling the `execute_search` function of the motion planner."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# solve for solution\n",
    "list_paths_primitives, _, _ = motion_planner.execute_search()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.3 Creating Trajectory object from the planning result\n",
    "The solutions returned by the motion planner is a list of List[State], which stores the states of the motion primitives constructing the solution trajectory. Note that the final state of a predecessor primitive is the same as the initial state of a successor primitive, thus we have to remove the duplicates in states."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad.scenario.trajectory import State, Trajectory\n",
    "from SMP.motion_planner.utility import create_trajectory_from_list_states\n",
    "\n",
    "trajectory_solution = create_trajectory_from_list_states(list_paths_primitives)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.4 Visualizing planned trajectory\n",
    "Given that we have constructed a feasible trajectory, we now visualize our solution (ego vehicle shown with green rectangle):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from SMP.motion_planner.utility import visualize_solution\n",
    "visualize_solution(scenario, planning_problem_set, trajectory_solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Checking Validity of Planning Results\n",
    "We provide a [Drivability Checker](https://commonroad.in.tum.de/drivability_checker) to verify the validity of the planned trajectories in terms of collision avoidance, kinematic feasibility, reaching goal state, etc. Here we demonstrate how to verify your solutions before saving and uploading them to the CommonRoad server. More information can be found at [Solution Checker](https://commonroad.in.tum.de/docs/commonroad-drivability-checker/sphinx/api_feasibility.html?highlight=valid_solution#module-commonroad_dc.feasibility.solution_checker)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.1 Creating Solution object\n",
    "Documentation regarding the **Solution** object can be found [here](https://commonroad-io.readthedocs.io/en/latest/api/common/#solution-class). You may refer to [Vehicle Models and Cost Functions](https://commonroad.in.tum.de/models_cost_functions) for more information. In this tutorial the Kinematic Single-Track Model is used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad.common.solution import Solution, PlanningProblemSolution, \\\n",
    "                                       VehicleModel, VehicleType, CostFunction\n",
    "\n",
    "# create PlanningProblemSolution object\n",
    "kwarg = {'planning_problem_id': planning_problem.planning_problem_id,\n",
    "         'vehicle_model':VehicleModel.KS,                            # used vehicle model, change if needed\n",
    "         'vehicle_type':VehicleType.BMW_320i,                        # used vehicle type, change if needed\n",
    "         'cost_function':CostFunction.SA1,                           # cost funtion, DO NOT use JB1\n",
    "         'trajectory':trajectory_solution}\n",
    "\n",
    "planning_problem_solution = PlanningProblemSolution(**kwarg)\n",
    "\n",
    "# create Solution object\n",
    "kwarg = {'scenario_id':scenario.scenario_id,\n",
    "         'planning_problem_solutions':[planning_problem_solution]}\n",
    "\n",
    "solution = Solution(**kwarg)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.2 Checking validity\n",
    "Now, we can easily verify the validity of our solution using function `valid_solution`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad_dc.feasibility.solution_checker import valid_solution\n",
    "\n",
    "valid_solution(scenario, planning_problem_set, solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If the first element of the above validation returns `True`, you can then safely save and submit your solutions to the CommonRoad server."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Saving Planning Results\n",
    "The final step is saving your solution trajectory to the planning problem into a CommonRoad XML file, which can later be uploaded to the [CommonRoad website](https://commonroad.in.tum.de/submissions/new) for the solutions to be ranked. [Here](https://commonroad-io.readthedocs.io/en/latest/api/common/#commonroadsolutionwriter-class) is a documentation regarding the **Solution Writer**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad.common.solution import CommonRoadSolutionWriter\n",
    "\n",
    "dir_output = \"../../outputs/solutions/\"\n",
    "\n",
    "# write solution to a CommonRoad XML file\n",
    "csw = CommonRoadSolutionWriter(solution)\n",
    "csw.write_to_file(output_path=dir_output, overwrite=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations!\n",
    "\n",
    "You have finished the tutorial on CommonRoad Search! You can already proceed with coming up with your own implementation of the motion planner. You can also proceed with the tutorial on motion primitive generator."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}