data_acquisition/data_labeling/TrajectoryVisualization.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Visualize Duckietown watchtower trajectory"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Importing and loading all the needed modules"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import contracts\n",
    "import yaml\n",
    "import numpy as np\n",
    "import geometry as geo\n",
    "import numpy as np\n",
    "from duckietown_world.world_duckietown.tile_template import load_tile_types\n",
    "from duckietown_world.geo.measurements_utils import iterate_by_class\n",
    "from duckietown_world.world_duckietown.tile import get_lane_poses\n",
    "from duckietown_world.svg_drawing.ipython_utils import ipython_draw_svg, ipython_draw_html\n",
    "import duckietown_world as dw\n",
    "from copy import deepcopy\n",
    "import geometry as g\n",
    "import pandas as pd\n",
    "import sys\n",
    "import traceback\n",
    "\n",
    "contracts.disable_all()\n",
    "m = dw.load_map('robotarium2')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Setting up necessary classes and functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Person(dw.PlacedObject):\n",
    "    \n",
    "    def __init__(self, radius, *args, **kwargs):\n",
    "        self.radius = radius\n",
    "        dw.PlacedObject.__init__(self, *args, **kwargs)\n",
    "\n",
    "    def draw_svg(self, drawing, g):\n",
    "        # drawing is done using the library svgwrite\n",
    "        c = drawing.circle(center=(0, 0), r=self.radius, fill='pink')\n",
    "        g.add(c)\n",
    "        # draws x,y axes\n",
    "        dw.draw_axes(drawing, g)\n",
    "\n",
    "    def extent_points(self):\n",
    "        # set of points describing the boundary \n",
    "        L = self.radius\n",
    "        return [(-L, -L), (+L, +L)]\n",
    "\n",
    "def relative_pose(q0, q1):\n",
    "    return g.SE2.multiply(g.SE2.inverse(q0), q1)\n",
    "\n",
    "def interpolate(q0, q1, alpha):\n",
    "    q1_from_q0 = relative_pose(q0, q1)\n",
    "    vel = g.SE2.algebra_from_group(q1_from_q0)\n",
    "    rel = g.SE2.group_from_algebra(vel * alpha)\n",
    "    q = g.SE2.multiply(q0, rel)\n",
    "    return q"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Importing trajectory files and extract relevant data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "root = dw.PlacedObject()\n",
    "\n",
    "realTimestamps = []\n",
    "seqs2 = []\n",
    "final_trajectory = []\n",
    "startTime = 0\n",
    "\n",
    "# Folder to trajectory for visualization\n",
    "folderNamesSingle = 'autobot04_r2'\n",
    "\n",
    "for fileName in range(0,500):\n",
    "    trajectoryFile = 'trajectoryFiles/' + str(folderNamesSingle) + '/' + str(folderNamesSingle) + '_' + str(fileName) + '.yaml'\n",
    "\n",
    "    try:\n",
    "        with open(trajectoryFile, 'r') as stream:\n",
    "            try:\n",
    "                data = yaml.safe_load(stream)\n",
    "\n",
    "            except yaml.YAMLError as exc:\n",
    "                print(exc)\n",
    "    except:\n",
    "        if fileName !=0:\n",
    "            break\n",
    "\n",
    "    timestart = data['begin_time_stamp']\n",
    "    data_points = len(data['trajectory_data'])\n",
    "\n",
    "    x = np.zeros((data_points,))\n",
    "    y= np.zeros((data_points,))\n",
    "    R = np.zeros((3,3, data_points))            \n",
    "    phi = np.zeros((3, data_points))\n",
    "\n",
    "    dx = 999.999*np.ones((data_points, ))\n",
    "    dy = 999.999*np.ones((data_points, ))\n",
    "\n",
    "    dr = 999.999*np.ones((data_points, ))\n",
    "    dphi = 999.999*np.ones((data_points, ))\n",
    "\n",
    "\n",
    "    for idx, [time, traj] in enumerate(data['trajectory_data'].items()):\n",
    "\n",
    "        x[idx] = np.array(traj[0])\n",
    "\n",
    "        y[idx] = np.array(traj[1])\n",
    "\n",
    "        R[:,:,idx] = np.reshape(np.array(traj[3:]), (3,3))\n",
    "\n",
    "\n",
    "        phi[:,idx] = np.array([np.arctan2(-R[1,2,idx],R[2,2,idx]), \n",
    "                               np.arctan2(R[0,2,idx],np.sqrt(R[0,0,idx]**2 + R[0,1,idx]**2)),\n",
    "                               np.arctan2(-R[0,1,idx], R[0,0,idx])])\n",
    "\n",
    "        realTimestamps.append(float(time) + float(timestart))\n",
    "        z = phi[2,idx]\n",
    "        points = np.array([x[idx], y[idx]])\n",
    "        final_trajectory.append([points, z])\n",
    "    \n",
    "final_array = final_trajectory\n",
    "\n",
    "for entry in range(0, len(final_array)):\n",
    "    x =  (final_array[entry][0][0] )  # -2.2\n",
    "    y = final_array[entry][0][1] # + 0.8\n",
    "    alpha = final_array[entry][1]\n",
    "    q5 = geo.SE2_from_translation_angle([x,y],alpha)\n",
    "    seqs2.append(q5)\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Calculate distance and relative heading to each timestamp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "timestamps = range(len(seqs2)) # [0, 1, 2, ...]\n",
    "\n",
    "# SE2Transform is the wrapper for SE2 used by Duckietown World \n",
    "transforms = [dw.SE2Transform.from_SE2(_) for _ in seqs2]\n",
    "seq_me = dw.SampledSequence(timestamps, transforms)\n",
    "counter = 0\n",
    "\n",
    "for timestamp, pose_object in seq_me:\n",
    "    print(counter)\n",
    "    lanePoses = list(get_lane_poses(m, pose_object.as_SE2()))\n",
    "    \n",
    "    if len(lanePoses) == 0:\n",
    "        print('')\n",
    "        \n",
    "    else:\n",
    "        distance_from_center = lanePoses[0].lane_pose.distance_from_center\n",
    "        rel_heading = lanePoses[0].lane_pose.relative_heading\n",
    "        tile = list(lanePoses[0].tile.children.keys())[0]\n",
    "        correctDir = lanePoses[0].lane_pose.correct_direction\n",
    "\n",
    "    counter += 1\n",
    "    print('')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Draw trajectory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load map\n",
    "m = dw.load_map('robotarium2')\n",
    "\n",
    "m.set_object(\"emded\", Person(0.1), ground_truth=seq_me)\n",
    "ipython_draw_html(m);\n"
   ]
  }
 ],
 "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}