000_Distance_Calculation_Notebook.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "c8c3fcd6",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Distance to line(left): 1.6955100822242166 m\n",
      "[25.21, 0.66, 25.198991424505184, 2.3554743437131527, 1.6955100822242166]\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import shapely.geometry as geom\n",
    "from shapely.geometry import Point\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "class NearestPoint(object):\n",
    "    def __init__(self, left_line, right_line, ax):\n",
    "        self.left_line = left_line\n",
    "        self.right_line = right_line\n",
    "        self.ax = ax\n",
    "        ax.figure.canvas.mpl_connect('button_press_event', self)\n",
    "\n",
    "    def __call__(self, xy_point, side, point_on_lane):\n",
    "        assert side == 'left' or side == 'right', 'Type among left or right'\n",
    "        distance = self.left_line.distance(xy_point)\n",
    "        print(f'Distance to line({side}):', distance, 'm')\n",
    "        \n",
    "        instant_resultList = [point_on_lane.y, -point_on_lane.x, distance]\n",
    "        return instant_resultList\n",
    "            \n",
    "    def draw_segment(self, object_list):\n",
    "        resultList = []\n",
    "        objectList = []\n",
    "        # determine the points whether those are at the left side or right side.\n",
    "        for i in object_list:\n",
    "            if i[0] <= 0:\n",
    "                left_point_list.append(Point(i[0], i[1]))\n",
    "            else:\n",
    "                right_point_list.append(Point(i[0], i[1]))\n",
    "        # Draw the lateral lane about the left objects\n",
    "        for point in left_point_list:\n",
    "            point_on_line = self.left_line.interpolate(self.left_line.project(point))\n",
    "            self.ax.plot([point.x, point_on_line.x], [point.y, point_on_line.y], \n",
    "                     color='red', marker='o', scalex=False, scaley=False)\n",
    "            fig.canvas.draw()\n",
    "            # get the distance in meter unit\n",
    "            resultList = resultList + [point.y, -point.x] + self.__call__(point, 'left', point_on_line)\n",
    "        # Draw the lateral lane about the right objects\n",
    "        for point in right_point_list:\n",
    "            point_on_line = self.right_line.interpolate(self.right_line.project(point))\n",
    "            self.ax.plot([point.x, point_on_line.x], [point.y, point_on_line.y], \n",
    "                     color='red', marker='o', scalex=False, scaley=False)\n",
    "            fig.canvas.draw()\n",
    "            # get the distance in meter unit\n",
    "            resultList = resultList + [point.y, -point.x] + self.__call__(point, 'right', point_on_line)\n",
    "        \n",
    "        return resultList\n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    # get the path of 3D points on lanes\n",
    "    l_coords = np.loadtxt('/home/kaai/chicago_ws/src/CSV_Communication/left_lane.txt')\n",
    "    r_coords = np.loadtxt('/home/kaai/chicago_ws/src/CSV_Communication/right_lane.txt')\n",
    "    left_line = geom.LineString(l_coords)\n",
    "    right_line = geom.LineString(r_coords)\n",
    "    \n",
    "    # define object locations\n",
    "    object_list = [[-0.66, 25.21],[-7.22, 38.47], [-4.43, 50.24]]\n",
    "    left_point_list = []\n",
    "    right_point_list = []\n",
    "    max_point = float(l_coords[l_coords.shape[0] - 1][1]) # indicate the maximum value from lane equation\n",
    "    # remove the object point which has bigger value than the maximum point\n",
    "    for i in range(len(object_list)):\n",
    "        if object_list[i][1] > max_point:\n",
    "            del object_list[i:]\n",
    "            break\n",
    "\n",
    "    # Plotting Section\n",
    "    fig, ax = plt.subplots()\n",
    "    ax.plot(*l_coords.T)\n",
    "    ax.plot(*r_coords.T)\n",
    "    ax.axis('equal')\n",
    "    # Set the frame\n",
    "    ax.set_xlim(-10, 10)\n",
    "    ax.set_ylim(-1, 45)\n",
    "    \n",
    "    # Make the class\n",
    "    distance_class = NearestPoint(left_line, right_line, ax)\n",
    "    # result = [x_value_on_lane, y_value_on_lane, distance]\n",
    "    result = distance_class.draw_segment(object_list)\n",
    "    \n",
    "    print(result)\n",
    "    \n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45e2694a",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "66ee2af5",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "prac_l = np.loadtxt('/home/kaai/chicago_ws/src/CSV_Communication/left_lane.txt')\n",
    "prac_r = np.loadtxt('/home/kaai/chicago_ws/src/CSV_Communication/right_lane.txt')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "c5043c22",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, 32.0, 33.0, 34.0, 35.0, 36.0, 37.0, 38.0]\n",
      "[2.3410404240722698, 2.336057875721889, 2.3315343460163223, 2.3274698349555694, 2.3238643425396304, 2.3207178687685053, 2.3180304136421936, 2.3158019771606964, 2.314032559324013, 2.312722160132143, 2.311870779585087, 2.311478417682845, 2.311545074425417, 2.312070749812803, 2.3130554438450024, 2.3144991565220154, 2.316401887843843, 2.318763637810484, 2.321584406421939, 2.324864193678208, 2.3286029995792905, 2.3328008241251874, 2.3374576673158978, 2.342573529151422, 2.34814840963176, 2.3541823087569123, 2.3606752265268782, 2.3676271629416576, 2.3750381180012514, 2.382908091705659, 2.3912370840548802, 2.4000250950489153, 2.4092721246877646, 2.4189781729714275, 2.429143239899904, 2.439767325473195, 2.4508504296912994, 2.462392552554218, 2.47439369406195]\n",
      "[0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, 32.0, 33.0, 34.0, 35.0, 36.0, 37.0, 38.0]\n",
      "[-2.2894339394472394, -2.270126919815131, -2.2515597374576095, -2.2337323923746752, -2.2166448845663282, -2.2002972140325685, -2.184689380773395, -2.1698213847888095, -2.1556932260788106, -2.142304904643399, -2.1296564204825743, -2.117747773596337, -2.1065789639846866, -2.096149991647623, -2.086460856585147, -2.0775115587972577, -2.0693020982839556, -2.061832475045241, -2.055102689081113, -2.049112740391572, -2.0438626289766186, -2.039352354836252, -2.0355819179704726, -2.03255131837928, -2.0302605560626747, -2.0287096310206563, -2.027898543253225, -2.027827292760381, -2.0284958795421244, -2.0299043035984545, -2.0320525649293715, -2.034940663534876, -2.038568599414967, -2.042936372569646, -2.0480439829989114, -2.0538914307027643, -2.060478715681204, -2.067805837934231, -2.0758727974618445]\n"
     ]
    }
   ],
   "source": [
    "left_lane_equat_point_x = []\n",
    "left_lane_equat_point_y = []\n",
    "right_lane_equat_point_x = []\n",
    "right_lane_equat_point_y = []\n",
    "\n",
    "for i in prac_l:\n",
    "    left_lane_equat_point_x.append(i[1])\n",
    "    left_lane_equat_point_y.append(-i[0])\n",
    "for i in prac_r:\n",
    "    right_lane_equat_point_x.append(i[1])\n",
    "    right_lane_equat_point_y.append(-i[0])\n",
    "    \n",
    "print(left_lane_equat_point_x)\n",
    "print(left_lane_equat_point_y)\n",
    "print(right_lane_equat_point_x)\n",
    "print(right_lane_equat_point_y)\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "62375cb7",
   "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.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}