lab11_real.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": []
   },
   "source": [
    "# Lab 11: Grid Localization using Bayes Filter (Real Robot)\n",
    "\n",
    "### <span style=\"color:rgb(0,150,0)\">It is recommended that you close any heavy-duty applications running on your system while working on this lab.</span>\n",
    "\n",
    "### <span style=\"color:rgb(0,150,0)\">The notebook only provides skeleton code for you to integrate the Localization class with the Real Robot.</span>\n",
    "\n",
    "<hr>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "\n",
    "import traceback\n",
    "from notebook_utils import *\n",
    "from Traj import *\n",
    "import asyncio\n",
    "import pathlib\n",
    "import os\n",
    "from utils import load_config_params\n",
    "from localization_extras import Localization\n",
    "\n",
    "# The imports below will only work if you copied the required ble-related python files \n",
    "# into the notebooks directory\n",
    "from ble import get_ble_controller\n",
    "from base_ble import LOG\n",
    "from cmd_types import CMD\n",
    "import numpy as np\n",
    "\n",
    "# Setup Logger\n",
    "LOG = get_logger('demo_notebook.log')\n",
    "LOG.propagate = False\n",
    "\n",
    "# Init GUI and Commander\n",
    "gui = GET_GUI()\n",
    "cmdr = gui.launcher.commander"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the plotter\n",
    "START_PLOTTER()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# The RealRobot class\n",
    "Define the RealRobot class in the code cell below, based on the documentation and your real robot communication protocol. <br>\n",
    "This class is used by the **Localization** class to communicate with the real robot. <br>\n",
    "More specifically, the **Localization** class utilizes the **RealRobot's** member function **perform_observation_loop()** to get the 18 sensor readings and store them in its member variable **obs_range_data**, which is then utilized in the update step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class RealRobot():\n",
    "    \"\"\"A class to interact with the real robot\n",
    "    \"\"\"\n",
    "    def __init__(self, commander, ble):\n",
    "        # Load world config\n",
    "        self.world_config = os.path.join(str(pathlib.Path(os.getcwd()).parent), \"config\", \"world.yaml\")\n",
    "        \n",
    "        self.config_params = load_config_params(self.world_config)\n",
    "        \n",
    "        # Commander to commuincate with the Plotter process\n",
    "        # Used by the Localization module to plot odom and belief\n",
    "        self.cmdr = commander\n",
    "\n",
    "        # ArtemisBLEController to communicate with the Robot\n",
    "        self.ble = ble\n",
    "\n",
    "    def get_pose(self):\n",
    "        \"\"\"Get robot pose based on odometry\n",
    "        \n",
    "        Returns:\n",
    "            current_odom -- Odometry Pose (meters, meters, degrees)\n",
    "        \"\"\"\n",
    "        raise NotImplementedError(\"get_pose is not implemented\")\n",
    "\n",
    "    def perform_observation_loop(self, rot_vel=120):\n",
    "        \"\"\"Perform the observation loop behavior on the real robot, where the robot does  \n",
    "        a 360 degree turn in place while collecting equidistant (in the angular space) sensor\n",
    "        readings, with the first sensor reading taken at the robot's current heading. \n",
    "        The number of sensor readings depends on \"observations_count\"(=18) defined in world.yaml.\n",
    "        \n",
    "        Keyword arguments:\n",
    "            rot_vel -- (Optional) Angular Velocity for loop (degrees/second)\n",
    "                        Do not remove this parameter from the function definition, even if you don't use it.\n",
    "        Returns:\n",
    "            sensor_ranges   -- A column numpy array of the range values (meters)\n",
    "            sensor_bearings -- A column numpy array of the bearings at which the sensor readings were taken (degrees)\n",
    "                               The bearing values are not used in the Localization module, so you may return a empty numpy array\n",
    "        \"\"\"\n",
    "        raise NotImplementedError(\"perform_observation_loop is not implemented\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get ArtemisBLEController object\n",
    "ble = get_ble_controller()\n",
    "\n",
    "# Connect to the Artemis Device\n",
    "ble.connect()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initialize RealRobot with a Commander object to communicate with the plotter process\n",
    "# and the ArtemisBLEController object to communicate with the real robot\n",
    "robot = RealRobot(cmdr, ble)\n",
    "\n",
    "# Initialize mapper\n",
    "# Requires a VirtualRobot object as a parameter\n",
    "mapper = Mapper(robot)\n",
    "\n",
    "# Initialize your BaseLocalization object\n",
    "# Requires a RealRobot object and a Mapper object as parameters\n",
    "loc = Localization(robot, mapper)\n",
    "\n",
    "## Plot Map\n",
    "cmdr.plot_map()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Run an update step of the Bayes Filter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Reset Plots\n",
    "cmdr.reset_plotter()\n",
    "\n",
    "# Init Uniform Belief\n",
    "loc.init_grid_beliefs()\n",
    "\n",
    "# Get Observation Data by executing a 360 degree rotation motion\n",
    "loc.get_observation_data()\n",
    "\n",
    "# Run Update Step\n",
    "loc.update_step()\n",
    "loc.plot_update_step_data(plot_data=True)\n",
    "\n",
    "# Plot Odom and GT\n",
    "# current_odom, current_gt = robot.get_pose()\n",
    "# cmdr.plot_gt(current_gt[0], current_gt[1])\n",
    "# cmdr.plot_odom(current_odom[0], current_odom[1])"
   ]
  }
 ],
 "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.10.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}