Jupyter Notebook.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Jupyter Notebook for the Final Assigment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first section is related to the import of all the libraries we need."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import rospy\n",
    "import time\n",
    "import numpy as np\n",
    "import ipywidgets as widgets\n",
    "from IPython.display import display\n",
    "from ipywidgets import Button, Layout\n",
    "from tf import transformations\n",
    "import matplotlib.pyplot as plt\n",
    "from nav_msgs.msg import Odometry\n",
    "from matplotlib.animation import FuncAnimation\n",
    "from sensor_msgs.msg import LaserScan\n",
    "from actionlib_msgs.msg import GoalStatusArray\n",
    "from IPython.display import clear_output\n",
    "\n",
    "import actionlib\n",
    "from geometry_msgs.msg import Twist\n",
    "from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal\n",
    "\n",
    "%matplotlib notebook\n",
    "%matplotlib notebook\n",
    "%matplotlib notebook\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Buttons Defination\n",
    "This section implements all buttons of the modality 1 and 2."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vel = Twist()\n",
    "third = False\n",
    "ok_left = True\n",
    "ok_straight = True\n",
    "ok_right = True\n",
    "\n",
    "vl = 1\n",
    "publ = rospy.Publisher('/cmd_vel', Twist, queue_size = 1)\n",
    "\n",
    "straight_button = widgets.Button(description = 'Up',\n",
    "                                 style=dict(font_style='italic',\n",
    "                                            font_weight='bold',\n",
    "                                            font_variant=\"small-caps\", \n",
    "                                            text_color='red', \n",
    "                                            text_decoration='underline')\n",
    "                                )\n",
    "\n",
    "back_button = widgets.Button(description = 'Down',\n",
    "                                 style=dict(font_style='italic',\n",
    "                                            font_weight='bold',\n",
    "                                            font_variant=\"small-caps\", \n",
    "                                            text_color='red', \n",
    "                                            text_decoration='underline')\n",
    "                                )\n",
    "\n",
    "left_button = widgets.Button(description = 'Left',\n",
    "                                 style=dict(font_style='italic',\n",
    "                                            font_weight='bold',\n",
    "                                            font_variant=\"small-caps\", \n",
    "                                            text_color='red', \n",
    "                                            text_decoration='underline')\n",
    "                                )\n",
    "\n",
    "\n",
    "right_button = widgets.Button(description = 'Right',\n",
    "                                 style=dict(font_style='italic',\n",
    "                                            font_weight='bold',\n",
    "                                            font_variant=\"small-caps\", \n",
    "                                            text_color='red', \n",
    "                                            text_decoration='underline')\n",
    "                                )\n",
    "\n",
    "increase_button = widgets.Button(description = 'Increase Velocities', \n",
    "                                 layout=Layout(width='60%', height='50px'), \n",
    "                                 button_style='info')\n",
    "increase_button.style.button_color = 'green'\n",
    "\n",
    "reset_button = widgets.Button(description = 'Reset Velocities', \n",
    "                              layout=Layout(width='60%', height='50px'), \n",
    "                              button_style='warning')\n",
    "\n",
    "stop_button = widgets.Button(description = 'Stop', \n",
    "                             layout=Layout(width='60%', height='50px'), \n",
    "                             button_style='danger') \n",
    "\n",
    "def straight_button_clicked(b):\n",
    "    global vel\n",
    "    global vl\n",
    "    global publ\n",
    "    \n",
    "    if ok_straight == True:\n",
    "        vel.linear.x = vl\n",
    "        vel.linear.y = 0\n",
    "        vel.linear.z = 0\n",
    "        vel.angular.x = 0\n",
    "        vel.angular.y = 0\n",
    "        vel.angular.z = 0\n",
    "        publ.publish(vel)\n",
    "    else:\n",
    "        print(\"Command 'Go Straight' disabled\")\n",
    "    \n",
    "straight_button.on_click(straight_button_clicked)\n",
    "\n",
    "def back_button_clicked(b):\n",
    "    global vel\n",
    "    global vl\n",
    "    global publ\n",
    "    \n",
    "    vel.linear.x = -vl\n",
    "    vel.linear.y = 0\n",
    "    vel.linear.z = 0\n",
    "    vel.angular.x = 0\n",
    "    vel.angular.y = 0\n",
    "    vel.angular.z = 0\n",
    "    publ.publish(vel)\n",
    "    \n",
    "back_button.on_click(back_button_clicked)  \n",
    "\n",
    "def left_button_clicked(b):\n",
    "    global vel\n",
    "    global vl\n",
    "    global publ\n",
    "    \n",
    "    if ok_left == True:\n",
    "        vel.linear.x = 0\n",
    "        vel.linear.y = 0\n",
    "        vel.linear.z = 0\n",
    "        vel.angular.x = 0\n",
    "        vel.angular.y = 0\n",
    "        vel.angular.z = vl\n",
    "        publ.publish(vel)\n",
    "    else:\n",
    "        print(\"Command 'Go Left' disabled\")\n",
    "\n",
    "left_button.on_click(left_button_clicked)\n",
    "    \n",
    "def right_button_clicked(b):\n",
    "    global vel\n",
    "    global vl\n",
    "    global publ\n",
    "\n",
    "    \n",
    "    if ok_right == True:\n",
    "        vel.linear.x = 0\n",
    "        vel.linear.y = 0\n",
    "        vel.linear.z = 0\n",
    "        vel.angular.x = 0\n",
    "        vel.angular.y = 0\n",
    "        vel.angular.z = -vl\n",
    "        publ.publish(vel)\n",
    "    else:\n",
    "        print(\"You can't go right.\")\n",
    "\n",
    "right_button.on_click(right_button_clicked)\n",
    "    \n",
    "def increase_v_button_clicked(b):\n",
    "    global vl\n",
    "    vl = vl + 1\n",
    "\n",
    "increase_button.on_click(increase_v_button_clicked)\n",
    "\n",
    "def reset_v_button_clicked(b):\n",
    "    global vl\n",
    "    vl = 1\n",
    "\n",
    "reset_button.on_click(reset_v_button_clicked)\n",
    "\n",
    "def stop_button_clicked(b):\n",
    "    vel.linear.x = 0\n",
    "    vel.linear.y = 0\n",
    "    vel.linear.z = 0\n",
    "    vel.angular.x = 0\n",
    "    vel.angular.y = 0\n",
    "    vel.angular.z = 0\n",
    "    publ.publish(vel)\n",
    "    \n",
    "stop_button.on_click(stop_button_clicked)\n",
    "\n",
    "row1=widgets.HBox([straight_button, left_button, back_button, right_button ])\n",
    "row3=widgets.HBox([stop_button])\n",
    "row4=widgets.HBox([increase_button])\n",
    "row5=widgets.HBox([reset_button])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Management of Modalities\n",
    "Here there's the development of the modalities in the jupyter enviroment. As you can see, for the third modality, I decided to sub to the LaserScan topic to get all the informations for avoiding the walls."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rospy.init_node(\"jupyter_ui\")\n",
    "output = widgets.Output()\n",
    "\n",
    "des_x = widgets.FloatText(\n",
    "    value=0,\n",
    "    description='X:',\n",
    "    disabled=False\n",
    ")\n",
    "des_y = widgets.FloatText(\n",
    "    value=0,\n",
    "    description='Y:',\n",
    "    disabled=False\n",
    ")\n",
    "\n",
    "boxes = widgets.HBox([des_x, des_y])\n",
    "\n",
    "cancel_button = widgets.Button(description = 'Click Me to cancel the goal!', layout=Layout(width='50%', height='80px'), button_style='danger')\n",
    "\n",
    "def cancel_button_clicked(b):\n",
    "    rospy.set_param('active', 0)\n",
    "    \n",
    "    \n",
    "cancel_button.on_click(cancel_button_clicked)\n",
    "    \n",
    "ok_button = widgets.Button(description = 'Send Goal', layout=Layout(width='30%', height='30px'), button_style='')\n",
    "\n",
    "def ok_button_clicked(b):\n",
    "\n",
    "    rospy.set_param('active', 0)\n",
    "    rospy.set_param('des_pos_x', des_x.value)\n",
    "    rospy.set_param('des_pos_y', des_y.value)\n",
    "    rospy.set_param('active', 1) \n",
    "    \n",
    "ok_button.on_click(ok_button_clicked)\n",
    "    \n",
    "button_first_mod = widgets.Button(description = 'First modality, put your goal coordinates!', layout=Layout(width='50%', height='80px'))\n",
    "button_first_mod.style.button_color = 'yellow'\n",
    "\n",
    "button_second_mod = widgets.Button(description = 'Second modality, move freely your robot!',\n",
    "                                   layout=Layout(width='50%', height='80px'))\n",
    "button_second_mod.style.button_color = 'orange'\n",
    "\n",
    "button_third_mod = widgets.Button(description = 'Third modality, move freely your robot assisted!', layout=Layout(width='50%', height='80px'), button_color='salmon')\n",
    "button_third_mod.style.button_color = 'red'\n",
    "\n",
    "\n",
    "def first_button_clicked(b):\n",
    "    back_to_menu_mod()\n",
    "    print(\"You chose the first controlling modality\")\n",
    "    third = False\n",
    "    display(des_x)\n",
    "    display(des_y)\n",
    "    display(ok_button)\n",
    "    display(cancel_button)\n",
    "    \n",
    "button_first_mod.on_click(first_button_clicked)\n",
    "\n",
    "def second_button_clicked(b):\n",
    "    back_to_menu_mod()\n",
    "    third = False\n",
    "    rospy.set_param('active', 0)\n",
    "    rospy.set_param('active', 2)\n",
    "    display(row1)\n",
    "    display(row3)\n",
    "    display(row4)\n",
    "    display(row5)\n",
    "    \n",
    "    \n",
    "button_second_mod.on_click(second_button_clicked)\n",
    "\n",
    "def clbk_laser(msg):\n",
    "    global vel\n",
    "    global ok_left\n",
    "    global ok_right\n",
    "    global ok_straight\n",
    "    global publ\n",
    "\n",
    "    right = min(min(msg.ranges[0:143]), 1)      # Detects obstacles on the right of the robot\n",
    "    front = min(min(msg.ranges[288:431]), 1)    # Detects obstacles in front of the robot\n",
    "    left = min(min(msg.ranges[576:719]), 1)     # Detects obstacles on the left of the robot\n",
    "\n",
    "    if right != 1.0 and vel.angular.z < 0:\n",
    "        ok_right =False\n",
    "        vel.angular.z = 0\n",
    "\n",
    "    else:         \n",
    "        ok_right =True\n",
    "\n",
    "    if front != 1.0 and vel.linear.x > 0:  \n",
    "        ok_straight = False\n",
    "        vel.linear.x = 0\n",
    "\n",
    "    else:              \n",
    "        ok_straight =True\n",
    "\n",
    "    if left != 1.0 and vel.angular.z > 0:    \n",
    "        ok_left =False\n",
    "        vel.angular.z = 0\n",
    "\n",
    "    else:            \n",
    "        ok_left =True\n",
    "        \n",
    "    publ.publish(vel)\n",
    "\n",
    "def third_button_clicked(b):\n",
    "    back_to_menu_mod()\n",
    "    third=True\n",
    "    rospy.set_param('active', 0)\n",
    "    rospy.set_param('active', 3)\n",
    "    display(row1)\n",
    "    display(row3)\n",
    "    display(row4)\n",
    "    display(row5)\n",
    "    \n",
    "button_third_mod.on_click(third_button_clicked)\n",
    "\n",
    "def back_to_menu_mod():\n",
    "    clear_output(wait=True)\n",
    "    display(buttons)\n",
    "\n",
    "sub = rospy.Subscriber('/scan', LaserScan, clbk_laser)\n",
    "buttons = widgets.HBox([button_first_mod, button_second_mod, button_third_mod])\n",
    "display(buttons)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Graphical Representation\n",
    "Here there's all the implementation of the graphs for showing the data of the robot. I imported again the libraries because sometimes some kind of bugs doesn't permit me to show the graphs. There are three graphs:\n",
    "    1. Position of the robot runtime.\n",
    "    2. Radar vision of the robot.\n",
    "    3. Goal status of the robot.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import rospy\n",
    "import time\n",
    "import numpy as np\n",
    "import ipywidgets as widgets\n",
    "from IPython.display import display\n",
    "from ipywidgets import Button, Layout\n",
    "import actionlib\n",
    "from geometry_msgs.msg import Twist\n",
    "from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal\n",
    "from tf import transformations\n",
    "import matplotlib.pyplot as plt\n",
    "from nav_msgs.msg import Odometry\n",
    "from matplotlib.animation import FuncAnimation\n",
    "from sensor_msgs.msg import LaserScan\n",
    "from actionlib_msgs.msg import GoalStatusArray\n",
    "\n",
    "\n",
    "angles = []\n",
    "\n",
    "class Visualiser:\n",
    "    def __init__(self):\n",
    "        \n",
    "        self.fig_laser = plt.figure(figsize=(6,6))\n",
    "        self.ax_laser = plt.subplot(111, polar=True)\n",
    "        self.ax_laser.set_thetalim(-np.pi/2,np.pi/2)\n",
    "        self.ax_laser.set_theta_zero_location(\"N\")\n",
    "       \n",
    "        self.ln_laser, = self.ax_laser.plot([],[],'bo')\n",
    "        \n",
    "        \n",
    "        self.fig_odom, self.ax_odom = plt.subplots()\n",
    "        plt.grid(True)\n",
    "        self.ln_odom, = plt.plot([] ,[], 'bo')\n",
    "        \n",
    "        self.achieved = False\n",
    "        self.goal_counter = 0\n",
    "        self.non_reached_goal_counter = 0\n",
    "        self.laser  = [] \n",
    "        self.x_pos, self.y_pos = [], []\n",
    "\n",
    "        self.fig_target, self.ax_target = plt.subplots()\n",
    "        plt.grid(True)\n",
    "        self.status_list = [0, 0]\n",
    "        self.data = {'Reached Goals': self.goal_counter, \n",
    "                     'Unreached Goals':self.non_reached_goal_counter, \n",
    "                     'Sent Goals': self.goal_counter + self.non_reached_goal_counter}\n",
    "        \n",
    "        self.target = list(self.data.keys())\n",
    "        self.values = list(self.data.values())\n",
    "        self.ln_target = plt.bar(self.target, self.values, color=(\"green\", \"red\", \"blue\"))\n",
    "        \n",
    "              \n",
    "    def plot_init(self):\n",
    "        self.ax_laser.set_title(\"Robot Laser\")\n",
    "        self.ax_laser.set_ylabel(\"Distance to obstacles\",fontweight=\"bold\")\n",
    "        \n",
    "        self.ax_odom.set_xlim(-20, 20)\n",
    "        self.ax_odom.set_ylim(-20, 20)\n",
    "\n",
    "        self.ax_odom.set_title(\"Robot Position\",fontweight=\"bold\")\n",
    "        self.ax_odom.set_ylabel(\"Y\",fontweight=\"bold\")\n",
    "        self.ax_odom.set_xlabel(\"X\",fontweight=\"bold\")\n",
    "\n",
    "        self.ax_target.set_ylim(0, 10)\n",
    "        \n",
    "        return self.ln_laser , self.ln_odom, self.ln_target\n",
    "    \n",
    "    def scan_callback(self, msg):\n",
    "        global angles\n",
    "        angles = list(np.arange(msg.angle_min, msg.angle_max+msg.angle_increment, msg.angle_increment))\n",
    "        self.laser=list(msg.ranges)\n",
    "        \n",
    "    def odom_callback(self, msg):\n",
    "        self.y_pos.append(msg.pose.pose.position.y)\n",
    "        self.x_pos.append(msg.pose.pose.position.x)\n",
    "        \n",
    "    def goal_cb(self, msg): \n",
    "\n",
    "        try: \n",
    "\n",
    "            self.status_list.append(msg.status_list[0].status)\n",
    "\n",
    "            if self.status_list[-1] == 3 and self.status_list[-2] != 3:\n",
    "                self.goal_counter = self.goal_counter + 1\n",
    "\n",
    "            elif self.status_list[-1] == 2 and self.status_list[-2] != 2:\n",
    "                self.non_reached_goal_counter = self.non_reached_goal_counter + 1\n",
    "\n",
    "            elif self.status_list[-1] == 4 and self.status_list[-2] != 2:\n",
    "                self.non_reached_goal_counter = self.non_reached_goal_counter + 1\n",
    "\n",
    "        except IndexError:\n",
    "            pass\n",
    "            \n",
    "            \n",
    "    def update_plot_odom(self, frame):\n",
    "        x = self.x_pos\n",
    "        y = self.y_pos\n",
    "        self.ln_odom.set_data(x, y)\n",
    "        return self.ln_odom\n",
    "    \n",
    "    def update_plot_target(self, frame):\n",
    "        x = self.goal_counter\n",
    "        y = self.non_reached_goal_counter\n",
    "        z = self.goal_counter + self.non_reached_goal_counter\n",
    "        self.updated_data = {'Reached Goals': x, \n",
    "                             'Non-reached Goals': y, \n",
    "                             'Total Sent Goals': z}\n",
    "        \n",
    "        h = list(self.updated_data.values())\n",
    "        q = list(self.updated_data.keys())\n",
    "        \n",
    "        self.ln_target = plt.bar(q, h, color=(\"green\", \"red\", \"blue\"))\n",
    "        \n",
    "        return self.ln_target\n",
    "    \n",
    "    def update_polar(self, frame):\n",
    "        global angles    \n",
    "        self.ax_laser.set_rmax(20)\n",
    "        self.ln_laser.set_data(angles,self.laser)\n",
    "        return self.ln_laser\n",
    "    \n",
    "\n",
    "vis = Visualiser()\n",
    "sub_scan = rospy.Subscriber(\"/scan\", LaserScan, vis.scan_callback)\n",
    "sub_odom = rospy.Subscriber(\"/odom\", Odometry, vis.odom_callback)\n",
    "sub_status = rospy.Subscriber(\"/move_base/status\", GoalStatusArray, vis.goal_cb)\n",
    "\n",
    "ani_laser = FuncAnimation(vis.fig_laser, vis.update_polar, blit = True)\n",
    "ani_odom = FuncAnimation(vis.fig_odom, vis.update_plot_odom, init_func=vis.plot_init)\n",
    "ani_target = FuncAnimation(vis.fig_target, vis.update_plot_target, init_func=vis.plot_init, interval=100)\n",
    "\n",
    "plt.show(block = True)\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}