rt2_robot_jupyter_ui.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "a71301ec",
   "metadata": {},
   "source": [
    "# Plot modality\n",
    "Choose the plot modality commenting/uncommenting the corresponding section."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9941e81f",
   "metadata": {},
   "source": [
    "## By the User Interface (UI)\n",
    "Plot the data directly by the User Interface: the plotting opens a new window and the kernel is busy (it is not possible continue to use the UI in the meanwhile) until it is closed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e6a8fc63",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:08.386335Z",
     "start_time": "2022-04-23T16:03:08.383994Z"
    }
   },
   "outputs": [],
   "source": [
    "#%matplotlib qt\n",
    "#mod_flag = 0"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c5e37485",
   "metadata": {},
   "source": [
    "## By the external cell \n",
    "Plot the data into the section 2: the plotting is in-line and it is possible continue to use the User Interface in the meanwhile."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4d2b9b9e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:08.693724Z",
     "start_time": "2022-04-23T16:03:08.392991Z"
    }
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%matplotlib notebook\n",
    "mod_flag = 1 "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e0357392",
   "metadata": {},
   "source": [
    "# Robot Interface (Code)\n",
    "Code to control the robot and plot data."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2f892f76",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "## Headers and Global variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c8cf8232",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.537746Z",
     "start_time": "2022-04-23T16:03:08.701307Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "# Robotics control in a simulated environment\n",
    "# Robotics Engineering\n",
    "# \n",
    "# @file rt2_rt2_jupyter_ui.ipynb\n",
    "# @author Simone Contorno (@simone-contorno)\n",
    "# @copyright Copyright (c) 2022\n",
    "\n",
    "import threading\n",
    "\n",
    "# Jupyter headers\n",
    "import jupyros as jr\n",
    "import ipywidgets as widgets\n",
    "from ipywidgets import Layout, Button, Box, VBox, HBox\n",
    "from IPython.display import display, clear_output\n",
    "\n",
    "# ROS headers\n",
    "import rospy\n",
    "import actionlib\n",
    "from geometry_msgs.msg import Twist\n",
    "from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionGoal\n",
    "\n",
    "# Visualiser headers\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "import tf\n",
    "from nav_msgs.msg import Odometry\n",
    "from sensor_msgs.msg import LaserScan\n",
    "from tf.transformations import quaternion_matrix\n",
    "from matplotlib.animation import FuncAnimation\n",
    "\n",
    "# Publishers\n",
    "pub_vel = rospy.Publisher(\"/cmd_vel\", Twist, queue_size = 1000)\n",
    "pub_goal = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size = 1000)\n",
    "\n",
    "# Widgets output\n",
    "send_out = widgets.Output()\n",
    "canc_out = widgets.Output()\n",
    "output = widgets.Output()\n",
    "\n",
    "# Global variables (coordinates and velocities)\n",
    "x = 0.0\n",
    "y = 0.0\n",
    "lin_vel = 1.0\n",
    "ang_vel = 0.0\n",
    "rate = 0.001 # Rate of 1 ms to update date in the plot\n",
    "\n",
    "# Flags \n",
    "hist_flag = 0\n",
    "goal_flag = 0\n",
    "assist_flag = 0\n",
    "\n",
    "# Ros parameter and object\n",
    "rospy.set_param('/goal_flag', goal_flag)\n",
    "robot_vel = Twist() \n",
    "\n",
    "# Button attributes\n",
    "button_layout = Layout(width = '300px')\n",
    "button_style = dict(\n",
    "    font_style = 'italic',\n",
    "    font_weight = 'bold',\n",
    "    font_variant = \"small-caps\",\n",
    "    text_color = 'red',\n",
    "    text_decoration = 'none'\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b0acab85",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "## Class Visualiser\n",
    "Class used to plot the robot position, the laser values and the reached/non-reached goal, dinamically."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d1105c1e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.555279Z",
     "start_time": "2022-04-23T16:03:09.544054Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Class to visualize data\n",
    "class Visualiser:    \n",
    "    def __init__(self):\n",
    "        self.fig, self.ax = plt.subplots()\n",
    "        self.fig.canvas.manager.set_window_title('Data plotting')\n",
    "        self.pl, = plt.plot([], [], 'ro')\n",
    "        self.pp, = plt.plot([], [], 'ro')\n",
    "        self.x_pos, self.y_pos = [] , []\n",
    "        self.x_scan, self.y_scan = [] , []\n",
    "    \n",
    "    def pp_init(self):\n",
    "        self.ax.set_xlim(-10, 10)\n",
    "        self.ax.set_ylim(-10, 10)\n",
    "        return self.pp\n",
    "    \n",
    "    def pl_init(self): \n",
    "        self.ax.set_xlim(-10, 730)\n",
    "        self.ax.set_ylim(0, 15)\n",
    "        return self.pl\n",
    "    \n",
    "    ## Update data\n",
    "    # Robot position\n",
    "    def update_odom(self, frame):\n",
    "        self.pp.set_data(self.x_pos, self.y_pos)\n",
    "        return self.pp\n",
    "    \n",
    "    # Laser scanner\n",
    "    def update_scan(self, frame):\n",
    "        self.pl.set_data(self.x_scan, self.y_scan)\n",
    "        return self.pl\n",
    "    \n",
    "    ## Callbacks\n",
    "    # Robot position\n",
    "    def odom_callback(self, msg):\n",
    "        if hist_flag == 0:\n",
    "            self.x_pos.clear();\n",
    "            self.y_pos.clear();\n",
    "        self.y_pos.append(msg.pose.pose.position.y)\n",
    "        self.x_pos.append(msg.pose.pose.position.x)\n",
    "    \n",
    "    # Laser scanner\n",
    "    def scan_callback(self, msg):\n",
    "        self.x_scan.clear();\n",
    "        self.y_scan.clear();\n",
    "        for i in range(len(msg.ranges)):\n",
    "            self.y_scan.append(msg.ranges[i])\n",
    "            self.x_scan.append(len(msg.ranges)-i)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d9468231",
   "metadata": {},
   "source": [
    "## Functions"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b642ff65",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "### General functions\n",
    "Functions used by the different interface modalities (main one, manual driving, plotting and new goal) of the UI."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "00eef5ec",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.564890Z",
     "start_time": "2022-04-23T16:03:09.558113Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Clear the button outputs\n",
    "def clear_out(arg):\n",
    "    output.clear_output()\n",
    "    send_out.clear_output()\n",
    "    canc_out.clear_output()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d6d4c9f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.571400Z",
     "start_time": "2022-04-23T16:03:09.566966Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Terminate the program\n",
    "def terminate(arg):\n",
    "    rospy.set_param('/goal_flag', 0)\n",
    "    clear_output() # Clear the cell output\n",
    "    print(\"\\033[1;30m\" + \"Bye.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c66d79e1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.579721Z",
     "start_time": "2022-04-23T16:03:09.573274Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Go back to the interface\n",
    "def back(arg):\n",
    "    clear_output() # Clear the cell output\n",
    "    interface()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6ae9d218",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.585933Z",
     "start_time": "2022-04-23T16:03:09.582641Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Enable driving assistance for the Menu interface\n",
    "def enable_driving_assistance(arg):\n",
    "    global assist_flag\n",
    "    assist_flag = 1\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/drive_flag', 1)\n",
    "    clear_output() # Clear the cell output\n",
    "    interface()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3b26e458",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.596874Z",
     "start_time": "2022-04-23T16:03:09.587764Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Disable driving assistance for the Menu interface\n",
    "def disable_driving_assistance(arg):\n",
    "    global assist_flag\n",
    "    assist_flag = 0\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/drive_flag', 0)\n",
    "    clear_output() # Clear the cell output\n",
    "    interface()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5b0ead7c",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "### Manual driving functions\n",
    "Functions used by the manual driving interface."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a9c33bd6",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.604415Z",
     "start_time": "2022-04-23T16:03:09.599598Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Enable driving assistance for the Manual driving interface\n",
    "def enable_driving_assistance_2(arg):\n",
    "    global assist_flag\n",
    "    assist_flag = 1\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/drive_flag', 1)\n",
    "    clear_output() # Clear the cell output\n",
    "    manual_driving(_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2ad78248",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.616803Z",
     "start_time": "2022-04-23T16:03:09.612421Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Disable driving assistance for the Manual driving interface\n",
    "def disable_driving_assistance_2(arg):\n",
    "    global assist_flag\n",
    "    assist_flag = 0\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/drive_flag', 0)\n",
    "    clear_output() # Clear the cell output\n",
    "    manual_driving(_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b5b89583",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.631174Z",
     "start_time": "2022-04-23T16:03:09.619971Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Go on \n",
    "def go_on(arg):\n",
    "    global lin_vel\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 0)\n",
    "    \n",
    "    robot_vel.linear.x = lin_vel.value\n",
    "    robot_vel.angular.z = 0\n",
    "    pub_vel.publish(robot_vel)\n",
    "    \n",
    "    with output:\n",
    "        output.clear_output()\n",
    "        if lin_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The linear velocity is set to 0.0: \" +\n",
    "                  \"the robot is not moving.\")\n",
    "        else: \n",
    "            print(\"\\033[1;34m\" + \"INFO: \" +\n",
    "                  \"\\033[0;30m\" + \"The robot is going on.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6acb1182",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.637382Z",
     "start_time": "2022-04-23T16:03:09.632656Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Go back\n",
    "def go_back(arg):\n",
    "    global lin_vel\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 3)\n",
    "    \n",
    "    robot_vel.linear.x = -lin_vel.value\n",
    "    robot_vel.angular.z = 0\n",
    "    pub_vel.publish(robot_vel)\n",
    "    \n",
    "    with output:\n",
    "        output.clear_output()\n",
    "        if lin_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The linear velocity is set to 0.0: \" +\n",
    "                  \"the robot is not moving.\")\n",
    "        else: \n",
    "            print(\"\\033[1;34m\" + \"INFO: \" +\n",
    "                  \"\\033[0;30m\" + \"The robot is going back.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8ecf8fd0",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.646008Z",
     "start_time": "2022-04-23T16:03:09.639562Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Curve left\n",
    "def curve_left(arg):\n",
    "    global lin_vel\n",
    "    global ang_vel\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 1)\n",
    "    \n",
    "    robot_vel.linear.x = lin_vel.value\n",
    "    robot_vel.angular.z = ang_vel.value\n",
    "    pub_vel.publish(robot_vel)\n",
    "    \n",
    "    with output:\n",
    "        output.clear_output()\n",
    "        if lin_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The linear velocity is set to 0.0: \" + \n",
    "                  \"the robot is going on.\")\n",
    "        elif ang_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The angular velocity is set to 0.0 \" + \n",
    "                  \"the robot is going on.\")\n",
    "        elif ang_vel.value == 0.0 and lin_vel.value == 0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The angular and the linear velocities are set to 0.0: \" + \n",
    "                  \"the robot is not moving.\")\n",
    "        else: \n",
    "            print(\"\\033[1;34m\" + \"INFO: \" +\n",
    "                  \"\\033[0;30m\" + \"The robot is curving left.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0237872a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.652901Z",
     "start_time": "2022-04-23T16:03:09.647883Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Curve right\n",
    "def curve_right(arg):\n",
    "    global lin_vel\n",
    "    global ang_vel\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 2)\n",
    "    \n",
    "    robot_vel.linear.x = lin_vel.value\n",
    "    robot_vel.angular.z = -ang_vel.value\n",
    "    pub_vel.publish(robot_vel)\n",
    "    \n",
    "    with output:\n",
    "        output.clear_output()\n",
    "        if lin_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The linear velocity is set to 0.0: \" + \n",
    "                  \"the robot is going on.\")\n",
    "        elif ang_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The angular velocity is set to 0.0 \" + \n",
    "                  \"the robot is going on.\")\n",
    "        elif ang_vel.value == 0.0 and lin_vel.value == 0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The angular and the linear velocities are set to 0.0: \" + \n",
    "                  \"the robot is not moving.\")\n",
    "        else: \n",
    "            print(\"\\033[1;34m\" + \"INFO: \" +\n",
    "                  \"\\033[0;30m\" + \"The robot is curving right.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "be4f66dd",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.663892Z",
     "start_time": "2022-04-23T16:03:09.659188Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Turn left\n",
    "def turn_left(arg):\n",
    "    global ang_vel\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 3)\n",
    "    \n",
    "    robot_vel.linear.x = 0.0\n",
    "    robot_vel.angular.z = ang_vel.value\n",
    "    pub_vel.publish(robot_vel)\n",
    "    \n",
    "    with output:\n",
    "        output.clear_output()\n",
    "        if ang_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The angular velocity is set to 0.0: \" + \n",
    "                  \"the robot is not moving.\")\n",
    "        else: \n",
    "            print(\"\\033[1;34m\" + \"INFO: \" +\n",
    "                  \"\\033[0;30m\" + \"The robot is turning left.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d594678",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.670731Z",
     "start_time": "2022-04-23T16:03:09.666389Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Turn right\n",
    "def turn_right(arg):\n",
    "    global ang_vel\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 3)\n",
    "    \n",
    "    robot_vel.linear.x = 0.0\n",
    "    robot_vel.angular.z = -ang_vel.value\n",
    "    pub_vel.publish(robot_vel)\n",
    "    \n",
    "    with output:\n",
    "        output.clear_output()\n",
    "        if ang_vel.value == 0.0:\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"The angular velocity is set to 0.0: \" + \n",
    "                  \"the robot is not moving.\")\n",
    "        else: \n",
    "            print(\"\\033[1;34m\" + \"INFO: \" +\n",
    "                  \"\\033[0;30m\" + \"The robot is turning right.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "23c5538f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.679985Z",
     "start_time": "2022-04-23T16:03:09.676217Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Stop\n",
    "def stop(arg):\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 3)\n",
    "    \n",
    "    robot_vel.linear.x = 0.0\n",
    "    robot_vel.angular.z = 0.0\n",
    "    pub_vel.publish(robot_vel)\n",
    "    \n",
    "    with output:\n",
    "        output.clear_output()\n",
    "        print(\"\\033[1;34m\" + \"INFO: \" +\n",
    "                  \"\\033[0;30m\" + \"The robot is not moving.\")   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6b4ac883",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.685055Z",
     "start_time": "2022-04-23T16:03:09.682184Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Quit manual driving \n",
    "def quit(arg):\n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    rospy.set_param('/key_flag', 3)\n",
    "    \n",
    "    robot_vel.linear.x = 0.0\n",
    "    robot_vel.angular.z = 0.0\n",
    "    pub_vel.publish(robot_vel)\n",
    "        \n",
    "    clear_output() # Clear the cell output\n",
    "    interface()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6e475921",
   "metadata": {},
   "source": [
    "### Manual driving interface\n",
    "Control the robot manually, setting the linear and the angular velocities, and using the buttons to choose the direction to run across."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dce50e9f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.704384Z",
     "start_time": "2022-04-23T16:03:09.691498Z"
    }
   },
   "outputs": [],
   "source": [
    "## Manual driving \n",
    "def manual_driving(arg):\n",
    "    clear_output() # Clear the cell output\n",
    "    print(\"\\033[1;31m\" + \"\\nManual driving\\n\")\n",
    "    print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "          \"\\033[0;30m\" + \"A linear velocity greater than 2.0, OR an Angular velocity \" +\n",
    "          \"greater than 5.0, could crash the simulation with RViz.\")\n",
    "    \n",
    "    global assist_flag \n",
    "    global lin_vel\n",
    "    global ang_vel\n",
    "    \n",
    "    rospy.set_param('/print_flag', 0)\n",
    "    goal_flag = rospy.get_param(\"/goal_flag\")\n",
    "    if goal_flag == 1:\n",
    "        rospy.set_param('/goal_flag', 0)\n",
    "        goal_flag = 0\n",
    "    \n",
    "    # Convert velocities type into float\n",
    "    if isinstance(lin_vel, widgets.widgets.widget_float.FloatSlider):\n",
    "        lin_vel = lin_vel.value\n",
    "    if isinstance(ang_vel, widgets.widgets.widget_float.FloatSlider):\n",
    "        ang_vel = ang_vel.value\n",
    "    \n",
    "    # Set linear and angular velocity\n",
    "    lin_vel = widgets.FloatSlider( # Robot linear velocity\n",
    "        value = lin_vel,\n",
    "        min = 0.0,\n",
    "        max = 2.0,\n",
    "        step = 0.1,\n",
    "        description = 'Linear velocity',\n",
    "        disabled = False,\n",
    "        continuous_update = False,\n",
    "        orientation = 'horizontal',\n",
    "        readout = True,\n",
    "        readout_format='.1f',\n",
    "    ) \n",
    "    ang_vel = widgets.FloatSlider( # Robot angular velocity\n",
    "        value = ang_vel,\n",
    "        min = 0.0,\n",
    "        max = 5.0,\n",
    "        step = 0.1,\n",
    "        description = 'Angular velocity',\n",
    "        disabled = False,\n",
    "        continuous_update = False,\n",
    "        orientation = 'horizontal',\n",
    "        readout = True,\n",
    "        readout_format='.1f',\n",
    "    )\n",
    "    \n",
    "    # Buttons\n",
    "    goon_b = widgets.Button(description = 'Go on', layout = button_layout, style = button_style)\n",
    "    goon_b.style.button_color = 'lightgreen'\n",
    "    \n",
    "    goback_b = widgets.Button(description = 'Go back', layout = button_layout, style = button_style)\n",
    "    goback_b.style.button_color = 'lightgreen'\n",
    "    \n",
    "    cleft_b = widgets.Button(description = 'Curve left', layout = button_layout, style = button_style)\n",
    "    cleft_b.style.button_color = 'lightgreen'\n",
    "    \n",
    "    cright_b = widgets.Button(description = 'Curve right', layout = button_layout, style = button_style)\n",
    "    cright_b.style.button_color = 'lightgreen'\n",
    "    \n",
    "    tleft_b = widgets.Button(description = 'Turn left', layout = button_layout, style = button_style)\n",
    "    tleft_b.style.button_color = 'lightgreen'\n",
    "    \n",
    "    tright_b = widgets.Button(description = 'Turn right', layout = button_layout, style = button_style)\n",
    "    tright_b.style.button_color = 'lightgreen'\n",
    "    \n",
    "    stop_b = widgets.Button(description = 'Stop', layout = button_layout, style = button_style)\n",
    "    stop_b.style.button_color = 'yellow'\n",
    "    \n",
    "    with_assist_b = widgets.Button(description = 'Enable driving assistance', layout = button_layout, style = button_style)\n",
    "    with_assist_b.style.button_color = 'lightgreen'\n",
    "    \n",
    "    without_assist_b = widgets.Button(description = 'Disable driving assistance', layout = button_layout, style = button_style)\n",
    "    without_assist_b.style.button_color = 'orange'\n",
    "    \n",
    "    quit_b = widgets.Button(description = 'Quit', layout = button_layout, style = button_style)\n",
    "    quit_b.style.button_color = 'red'\n",
    "    \n",
    "    # Buttons' function\n",
    "    goon_b.on_click(go_on)\n",
    "    goback_b.on_click(go_back)\n",
    "    cleft_b.on_click(curve_left)\n",
    "    cright_b.on_click(curve_right)\n",
    "    tleft_b.on_click(turn_left)\n",
    "    tright_b.on_click(turn_right)\n",
    "    stop_b.on_click(stop)\n",
    "    with_assist_b.on_click(enable_driving_assistance_2)\n",
    "    without_assist_b.on_click(disable_driving_assistance_2)\n",
    "    quit_b.on_click(back)\n",
    "    \n",
    "    # Display the buttons as a box\n",
    "    if assist_flag == 0:\n",
    "        display(lin_vel, ang_vel, \n",
    "            widgets.HBox((cleft_b, goon_b, cright_b)), \n",
    "            widgets.HBox((tleft_b, goback_b, tright_b,)),\n",
    "            widgets.HBox((stop_b, with_assist_b, quit_b)), \n",
    "            output)\n",
    "    elif assist_flag == 1:\n",
    "        display(lin_vel, ang_vel, \n",
    "            widgets.HBox((cleft_b, goon_b, cright_b)), \n",
    "            widgets.HBox((tleft_b, goback_b, tright_b,)),\n",
    "            widgets.HBox((stop_b, without_assist_b, quit_b)), \n",
    "            output)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e6921b40",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "### Goal management functions\n",
    "Function used by the goal management interface. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b99764d9",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.712981Z",
     "start_time": "2022-04-23T16:03:09.708114Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Cancel the current goal\n",
    "def cancel_goal(arg):\n",
    "    goal_flag = rospy.get_param(\"/goal_flag\")\n",
    "    if goal_flag == 1:\n",
    "        rospy.set_param('/goal_flag', 0)\n",
    "        goal_flag = 0\n",
    "        \n",
    "        with canc_out:    \n",
    "            canc_out.clear_output()\n",
    "            print(\"\\033[1;34m\" + \"INFO: \" + \n",
    "                  \"\\033[0;30m\" + \"Goal cancelled.\")\n",
    "    else: \n",
    "        with canc_out:    \n",
    "            canc_out.clear_output()\n",
    "            print(\"\\033[1;33m\" + \"WARNING: \" + \n",
    "                  \"\\033[0;30m\" + \"There is no goal set.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1ffc4544",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.723296Z",
     "start_time": "2022-04-23T16:03:09.714709Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Send the new coordinates\n",
    "def send_coordinates(arg):\n",
    "    goal_pos = MoveBaseActionGoal()    \n",
    "    goal_pos.goal.target_pose.header.frame_id = \"map\"\n",
    "    goal_pos.goal.target_pose.pose.orientation.w = 1;\n",
    "    goal_pos.goal.target_pose.pose.position.x = x.value\n",
    "    goal_pos.goal.target_pose.pose.position.y = y.value\n",
    "\n",
    "    # Publish\n",
    "    pub_goal.publish(goal_pos)\n",
    "    rospy.set_param('/goal_flag', 1)\n",
    "    \n",
    "    with send_out:    \n",
    "        send_out.clear_output()\n",
    "        print(\"\\033[1;34m\" + \"INFO: \" + \n",
    "              \"\\033[0;30m\" + \"Goal sent.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f41ea5fb",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "### Goal management interface\n",
    "Insert the new coordinates to reach automatically with the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e3f2d0ac",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.729792Z",
     "start_time": "2022-04-23T16:03:09.725163Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Set new coordinated\n",
    "def set_coordinates(arg):\n",
    "    clear_output() # Clear the cell output\n",
    "    print(\"\\033[1;31m\" + \"\\nAutonomous driving\\n\")\n",
    "    print(\"\\033[1;30m\" + \"Set the coordinates to reach:\")\n",
    "    \n",
    "    send_b = widgets.Button(description = 'Send', layout = button_layout, style = button_style) \n",
    "    send_b.style.button_color = 'lightgreen'\n",
    "    back_b = widgets.Button(description = 'Back', layout = button_layout, style = button_style)\n",
    "    back_b.style.button_color = 'red'\n",
    "    \n",
    "    global x\n",
    "    global y \n",
    "    \n",
    "    x = widgets.BoundedFloatText(\n",
    "    value = 0.0,\n",
    "    min = -100,\n",
    "    step = 0.1,\n",
    "    description = 'x :',\n",
    "    disabled = False\n",
    "    )\n",
    "    y = widgets.BoundedFloatText(\n",
    "    value = 0.0,\n",
    "    min = -100,\n",
    "    step = 0.1,\n",
    "    description = 'y :',\n",
    "    disabled = False\n",
    "    )\n",
    "    \n",
    "    back_b.on_click(back)\n",
    "    send_b.on_click(send_coordinates)\n",
    "    \n",
    "    display(x, y, send_b, back_b, send_out)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "63c65822",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "### Plot functions\n",
    "Functions used by the plot interface."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ec5cf42",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.739938Z",
     "start_time": "2022-04-23T16:03:09.731121Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Plot the number of reached/non-reached targets\n",
    "def plot_target(arg):\n",
    "    reached_goal = rospy.get_param(\"/reached_goal\")\n",
    "    non_reached_goal = rospy.get_param(\"/non_reached_goal\")\n",
    "    targets = [\"Reached\", \"Non-reached\"]\n",
    "    number = [reached_goal, non_reached_goal]\n",
    "    \n",
    "    fig = plt.figure()\n",
    "    fig.canvas.manager.set_window_title('Data plotting')\n",
    "    plt.bar(targets, number, color ='blue', width = 0.5)\n",
    "    plt.title(\"Targets info\")\n",
    "    plt.xlabel(\"Targets\")\n",
    "    plt.ylabel(\"Number\")\n",
    "    plt.show(block = False)\n",
    "    \n",
    "    while plt.fignum_exists(1):\n",
    "        plt.pause(rate) \n",
    "        \n",
    "        # Update the histogram\n",
    "        new_reached_goal = rospy.get_param(\"/reached_goal\")\n",
    "        new_non_reached_goal = rospy.get_param(\"/non_reached_goal\")\n",
    "        if (new_reached_goal != reached_goal) or (new_non_reached_goal != non_reached_goal):\n",
    "            reached_goal = new_reached_goal\n",
    "            non_reached_goal = new_non_reached_goal\n",
    "            number = [reached_goal, non_reached_goal]\n",
    "            plt.bar(targets, number, color ='blue', width = 0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81a81c59",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.746089Z",
     "start_time": "2022-04-23T16:03:09.741778Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Plot laser scanner data\n",
    "def plot_laserscan(arg):\n",
    "    vis = Visualiser() \n",
    "    ani = FuncAnimation(vis.fig, \n",
    "                        vis.update_scan, \n",
    "                        init_func = vis.pl_init)\n",
    "    plt.title(\"Laser scanner\")\n",
    "    plt.ylabel(\"Distance\")\n",
    "    sub = rospy.Subscriber('/scan', LaserScan, vis.scan_callback)\n",
    "    plt.show(block = False)\n",
    "    \n",
    "    while plt.fignum_exists(1):\n",
    "        plt.pause(rate)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2593434b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.754672Z",
     "start_time": "2022-04-23T16:03:09.747338Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Plot robot position data\n",
    "def plot_position(arg):\n",
    "    vis = Visualiser() \n",
    "    ani = FuncAnimation(vis.fig, \n",
    "                        vis.update_odom, \n",
    "                        init_func = vis.pp_init)\n",
    "    plt.title(\"Robot position\")\n",
    "    plt.xlabel(\"x\")\n",
    "    plt.ylabel(\"y\")\n",
    "    sub = rospy.Subscriber('/odom', Odometry, vis.odom_callback)\n",
    "    plt.show(block = False)\n",
    "    \n",
    "    while plt.fignum_exists(1):\n",
    "        plt.pause(rate)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5b8a3b0f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.762377Z",
     "start_time": "2022-04-23T16:03:09.757349Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Plot robot position and laser scanner data\n",
    "def plot_pos_and_scan(arg):\n",
    "    vis1 = Visualiser() \n",
    "    ani1 = FuncAnimation(vis1.fig, \n",
    "                        vis1.update_odom, \n",
    "                        init_func = vis1.pp_init)\n",
    "    plt.title(\"Robot position\")\n",
    "    plt.xlabel(\"x\")\n",
    "    plt.ylabel(\"y\")\n",
    "    sub = rospy.Subscriber('/odom', Odometry, vis1.odom_callback)\n",
    "    plt.show(block = False)\n",
    "    \n",
    "    vis2 = Visualiser() \n",
    "    ani2 = FuncAnimation(vis2.fig, \n",
    "                        vis2.update_scan, \n",
    "                        init_func = vis2.pl_init)\n",
    "    plt.title(\"Laser scanner\")\n",
    "    plt.ylabel(\"Distance\")\n",
    "    sub = rospy.Subscriber('/scan', LaserScan, vis2.scan_callback)\n",
    "    plt.show(block = False)\n",
    "\n",
    "    while plt.fignum_exists(1) or plt.fignum_exists(2):\n",
    "        plt.pause(rate)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d0cf237",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.768477Z",
     "start_time": "2022-04-23T16:03:09.763968Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Enable position history\n",
    "def enable_pos_hist(arg):\n",
    "    global hist_flag\n",
    "    hist_flag = 1\n",
    "    data_visualization(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "db0c6da9",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.779169Z",
     "start_time": "2022-04-23T16:03:09.776895Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Disable position history\n",
    "def disable_pos_hist(arg):\n",
    "    global hist_flag\n",
    "    hist_flag = 0\n",
    "    data_visualization(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "61c75c59",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "### Plot interface\n",
    "Plot the the robot position, the laser values and the reached/non-reached goal, dinamically."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "43b204c3",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.791770Z",
     "start_time": "2022-04-23T16:03:09.780942Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Visualize data about robot position\n",
    "def data_visualization(arg):  \n",
    "    global hist_flag\n",
    "    clear_output() # Clear the cell output\n",
    "    \n",
    "    # Choose the data to plot\n",
    "    print(\"\\033[1;31m\" + \"\\nData plotting\\n\")\n",
    "    print(\"\\033[1;30m\" + \"Choose the data that you want to plot:\")\n",
    "    \n",
    "    ## Buttons\n",
    "    \n",
    "    # Position button\n",
    "    pos_b = widgets.Button(description = 'Plot position data', layout = button_layout, style = button_style)\n",
    "    pos_b.style.button_color = 'lightgreen'\n",
    "    pos_b.on_click(plot_position) # function\n",
    "    \n",
    "    # Laser button\n",
    "    scan_b = widgets.Button(description = 'Plot laser scanner data', layout = button_layout, style = button_style)\n",
    "    scan_b.style.button_color = 'lightgreen'\n",
    "    scan_b.on_click(plot_laserscan) # function\n",
    "    \n",
    "    # Position and laser button\n",
    "    pos_scan_b = widgets.Button(description = 'Plot position and laser scanner data', layout = button_layout, style = button_style)\n",
    "    pos_scan_b.style.button_color = 'lightgreen'\n",
    "    pos_scan_b.on_click(plot_pos_and_scan) # function\n",
    "    \n",
    "    # Number of reached/non-reached targets button\n",
    "    target_b = widgets.Button(description = 'Plot targets info', layout = button_layout, style = button_style)\n",
    "    target_b.style.button_color = 'lightgreen'\n",
    "    target_b.on_click(plot_target) # function\n",
    "    \n",
    "    # Enable history button\n",
    "    with_hist_b = widgets.Button(description = 'Enable position history', layout = button_layout, style = button_style)\n",
    "    with_hist_b.style.button_color = 'lightgreen'\n",
    "    with_hist_b.on_click(enable_pos_hist) # function\n",
    "    \n",
    "    # Disable history button\n",
    "    without_hist_b = widgets.Button(description = 'Disable position history', layout = button_layout, style = button_style)\n",
    "    without_hist_b.style.button_color = 'orange'\n",
    "    without_hist_b.on_click(disable_pos_hist) # function\n",
    "    \n",
    "    # Back button\n",
    "    back_b = widgets.Button(description = 'Back', layout = button_layout, style = button_style)\n",
    "    back_b.style.button_color = 'red'\n",
    "    back_b.on_click(back) # function \n",
    "    \n",
    "    # Display buttons\n",
    "    if hist_flag == 0:\n",
    "        display(pos_b, scan_b, target_b, pos_scan_b, with_hist_b, back_b)    \n",
    "    elif hist_flag == 1:\n",
    "        display(pos_b, scan_b, target_b, pos_scan_b, without_hist_b, back_b)   "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a551eda3",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "### User Interface (UI)\n",
    "Main UI: terminate the program, plot data, insert new goal, cancel the current goal, pass to the manual driving and enable/disable the driving assistance."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7a18f16b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:09.806923Z",
     "start_time": "2022-04-23T16:03:09.793499Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "## Show the UI\n",
    "def interface():\n",
    "    global assist_flag\n",
    "    global mod_flag \n",
    "    \n",
    "    # Choose the action to perform\n",
    "    print(\"\\033[1;31m\" + \"\\nWelcome to the User Interface!\\n\")\n",
    "    print(\"\\033[1;30m\" + \"Here you can choose between two different modalities to control your robot:\\n\" \n",
    "          \"- automatic goal reaching, or\\n\"\n",
    "          \"- manual driving (with or without the driving assistance!)\\n\"\n",
    "          \"Choose an action:\")\n",
    "    \n",
    "    ## Buttons\n",
    "    \n",
    "    # Exit button\n",
    "    exit_b = widgets.Button(description = 'Exit', layout = button_layout, style = button_style)\n",
    "    exit_b.style.button_color = 'red'\n",
    "    exit_b.on_click(terminate) # function\n",
    "    \n",
    "    # Plot data button\n",
    "    plot_b = widgets.Button(description = 'Plot data', layout = button_layout, style = button_style)\n",
    "    plot_b.style.button_color = 'lightgreen'\n",
    "    plot_b.on_click(data_visualization) # function\n",
    "    \n",
    "    # Insert new goal button\n",
    "    goal_b = widgets.Button(description = 'Insert new coordinates to reach', layout = button_layout, style = button_style)\n",
    "    goal_b.style.button_color = 'lightgreen'\n",
    "    goal_b.on_click(set_coordinates) # function\n",
    "    \n",
    "    # Cancel current goal button\n",
    "    canc_b = widgets.Button(description = 'Cancel the current goal', layout = button_layout, style = button_style)\n",
    "    canc_b.style.button_color = 'lightgreen'\n",
    "    canc_b.on_click(cancel_goal) # function\n",
    "    \n",
    "    # Manual driving button\n",
    "    drive_b = widgets.Button(description = 'Manual driving', layout = button_layout, style = button_style)\n",
    "    drive_b.style.button_color = 'lightgreen'\n",
    "    drive_b.on_click(manual_driving) # function\n",
    "    \n",
    "    # Enable driving assistance button\n",
    "    with_assist_b = widgets.Button(description = 'Enable driving assistance', layout = button_layout, style = button_style)\n",
    "    with_assist_b.style.button_color = 'lightgreen'\n",
    "    with_assist_b.on_click(enable_driving_assistance) # function\n",
    "    \n",
    "    # Disable driving assistance button\n",
    "    without_assist_b = widgets.Button(description = 'Disable driving assistance', layout = button_layout, style = button_style)\n",
    "    without_assist_b.style.button_color = 'orange'\n",
    "    without_assist_b.on_click(disable_driving_assistance) # function\n",
    "    \n",
    "    # Clear buttons' output button\n",
    "    clear_b = widgets.Button(description = 'Clear button outputs', layout = button_layout, style = button_style)\n",
    "    clear_b.style.button_color = 'yellow'\n",
    "    clear_b.on_click(clear_out) # function\n",
    "    \n",
    "    # Display buttons\n",
    "    \n",
    "    if mod_flag == 0:\n",
    "        if assist_flag == 0:\n",
    "            display(exit_b, plot_b, goal_b, canc_b, canc_out, drive_b, with_assist_b, clear_b)   \n",
    "        elif assist_flag == 1:\n",
    "            display(exit_b, plot_b, goal_b, canc_b, canc_out, drive_b, without_assist_b, clear_b)\n",
    "    elif mod_flag == 1:\n",
    "        if assist_flag == 0:\n",
    "            display(exit_b, goal_b, canc_b, canc_out, drive_b, with_assist_b, clear_b)   \n",
    "        elif assist_flag == 1:\n",
    "            display(exit_b, goal_b, canc_b, canc_out, drive_b, without_assist_b, clear_b)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3a800863",
   "metadata": {},
   "source": [
    "# Robot Interface (UI)\n",
    "Main function: initialize the node and run the UI. <br>\n",
    "In order to plot the data through the UI: <br>\n",
    "1) Uncomment the section 1.1 <br>\n",
    "2) Comment the section 1.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b862b175",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:10.013380Z",
     "start_time": "2022-04-23T16:03:09.809027Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "## main function to run the user interface\n",
    "def main():\n",
    "    rospy.init_node(\"rt2_robot_jupyter_ui\")    \n",
    "    interface()\n",
    "    \n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d4b94b59",
   "metadata": {},
   "source": [
    "# Plot Interface (external cell)\n",
    "Plot the data into this external cell: <br>\n",
    "1) Comment the section 1.1 <br>\n",
    "2) Uncomment the section 1.2"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "70147371",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "## Update target function\n",
    "Update the plot of reached/non-reached targets."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "59bf0471",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:10.018326Z",
     "start_time": "2022-04-23T16:03:10.015264Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "# Update targets plot\n",
    "def update_target(arg):\n",
    "    reached_goal = rospy.get_param(\"/reached_goal\")\n",
    "    non_reached_goal = rospy.get_param(\"/non_reached_goal\")\n",
    "    targets = [\"Reached\", \"Non-reached\"]\n",
    "    number = [reached_goal, non_reached_goal]\n",
    "    \n",
    "    plt.bar(targets, number, color ='blue', width = 0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "193c1426",
   "metadata": {
    "heading_collapsed": true
   },
   "source": [
    "## Robot position history\n",
    "Comment/Uncomment this cell to enable/disable the robot position history during its plot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "466db194",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:10.028830Z",
     "start_time": "2022-04-23T16:03:10.021012Z"
    },
    "hidden": true
   },
   "outputs": [],
   "source": [
    "#hist_flag = 1 # uncomment this line to keep track of the robot movements"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "be21dca3",
   "metadata": {},
   "source": [
    "## Robot position\n",
    "Plot the robot position data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea29a9f6",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:10.079206Z",
     "start_time": "2022-04-23T16:03:10.032592Z"
    }
   },
   "outputs": [],
   "source": [
    "# Verify if the plot modality is enabled\n",
    "if mod_flag == 1:\n",
    "    \n",
    "    ## Plot robot position data\n",
    "    vis1 = Visualiser() \n",
    "    ani1 = FuncAnimation(vis1.fig, \n",
    "                        vis1.update_odom, \n",
    "                        init_func = vis1.pp_init)\n",
    "    plt.title(\"Robot position\")\n",
    "    plt.xlabel(\"x\")\n",
    "    plt.ylabel(\"y\")\n",
    "    sub = rospy.Subscriber('/odom', Odometry, vis1.odom_callback)\n",
    "    plt.show(block = False)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dc3d7cc2",
   "metadata": {},
   "source": [
    "## Laser scanner\n",
    "Plot the laser scanner data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "12d86fbe",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:10.130198Z",
     "start_time": "2022-04-23T16:03:10.081290Z"
    }
   },
   "outputs": [],
   "source": [
    "# Verify if the plot modality is enabled\n",
    "if mod_flag == 1:\n",
    "    \n",
    "    ## Plot laser scanner data\n",
    "    vis2 = Visualiser() \n",
    "    ani2 = FuncAnimation(vis2.fig, \n",
    "                        vis2.update_scan, \n",
    "                        init_func = vis2.pl_init)\n",
    "    plt.title(\"Laser scanner\")\n",
    "    plt.ylabel(\"Distance\")\n",
    "    sub = rospy.Subscriber('/scan', LaserScan, vis2.scan_callback)\n",
    "    plt.show(block = False)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "db7ce57c",
   "metadata": {},
   "source": [
    "## Number of reached/non-reached targets\n",
    "Plot the number of reached/non-reached targets."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fd9721be",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:10.189995Z",
     "start_time": "2022-04-23T16:03:10.132278Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# Verify if the plot modality is enabled\n",
    "if mod_flag == 1:\n",
    "    \n",
    "    ## Plot the number of reached/non-reached targets\n",
    "    reached_goal = rospy.get_param(\"/reached_goal\")\n",
    "    non_reached_goal = rospy.get_param(\"/non_reached_goal\")\n",
    "    targets = [\"Reached\", \"Non-reached\"]\n",
    "    number = [reached_goal, non_reached_goal]\n",
    "\n",
    "    fig = plt.figure()\n",
    "    fig.canvas.manager.set_window_title('Data plotting')\n",
    "    plt.bar(targets, number, color ='blue', width = 0.5)\n",
    "    plt.title(\"Targets info\")\n",
    "    plt.xlabel(\"Targets\")\n",
    "    plt.ylabel(\"Number\")\n",
    "    plt.show(block = False)\n",
    "    \n",
    "    target_b = widgets.Button(description = 'Update targets plot', layout = button_layout, style = button_style)\n",
    "    target_b.style.button_color = 'lightgreen'\n",
    "    target_b.on_click(update_target) # function\n",
    "    \n",
    "    display(target_b)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d04c3656",
   "metadata": {},
   "source": [
    "# 3D map\n",
    "Show the 3D map in which the robot moves."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cbd1ffcf",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-04-23T16:03:10.228338Z",
     "start_time": "2022-04-23T16:03:10.192149Z"
    }
   },
   "outputs": [],
   "source": [
    "## Visualize the 3D map, the robot and the laser scanner\n",
    "\n",
    "# Jupyter headers\n",
    "import os\n",
    "from jupyros import ros3d\n",
    "\n",
    "view = ros3d.Viewer() # ROS 3D object\n",
    "rc = ros3d.ROSConnection(url = \"ws://localhost:9090\") # Websocket (9090:rosbridge)\n",
    "tf_client = ros3d.TFClient(ros = rc, fixed_frame = 'map') # Client connection \n",
    "\n",
    "# Visualize the map, the laser and the path to follow\n",
    "map_view = ros3d.OccupancyGrid(topic = \"/map\", ros = rc, tf_client = tf_client) \n",
    "laser_view = ros3d.LaserScan(topic = \"/scan\", ros = rc, tf_client = tf_client) \n",
    "path = ros3d.Path(topic = \"/move_base/NavfnROS/plan\", ros = rc, tf_client = tf_client)\n",
    "\n",
    "urdf = ros3d.URDFModel(ros = rc, tf_client = tf_client,\n",
    "    path = os.environ.get('JUPYROS_ASSETS_URL', 'http://localhost:3000'))\n",
    "\n",
    "g = ros3d.GridModel() # Grid object\n",
    "\n",
    "# Show the model\n",
    "view.objects = [g, laser_view, map_view, path, urdf]\n",
    "view"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "23bbf2bf",
   "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.10"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}