Course_RvizMarkers_Unit_3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 3: Add Overlays"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Sometimes, it's necessary to have some kind of user friendly display of the important data in RVIZ. Like, for example, the speed of the robot base, or CPU load, or maybe the learning index. Maybe you need to know the effort made by a robot hand and detect when it exceeded a certain limit.<br>\n",
    "You also may need to indicate in which state the robot is in now: Is it learning? Processing? Lost?<br>\n",
    "All this and much more can be obtained with overlays."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Adding Plots, PieCharts, and Menus"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you are going to learn how the demo overlays were made through an example:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, essentially, you have two types of overlays here:<br>\n",
    "<ul>\n",
    "<li>Generated reading only topics: The <b>Plot</b> and the <b>PieChart</b> are like this. You just have to add the element to RVIZ and tell it what topic is publishing the float32 data you want it to read. You don't need to do anything else.</li>\n",
    "<li>Generated with Objects and publishing those markers: This is the case of the <b>OverlayText</b> and <b>OverlayMenu</b></li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Plots and PieCharts"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Just add both elements to an RVIZ session:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/rviz_marker_unit3_plot.png\"/>\n",
    "<img src=\"img/rviz_marker_unit3_piechart.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can then add a new Panel with the plus sign and select the <b>Overlay Picker</b>. This will allow you to move the Plots and PieCharts around where you want, and when you save the RVIZ session, they will stay where you left them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/rviz_marker_unit3_overlaypicker.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Menus and TextOverlay"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And to add menus and text functionality, we will have to create objects as usual. Here you have the basics:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### OverlayText"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "text_pub = rospy.Publisher(\"/text_sample\", OverlayText, queue_size=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create the publisher, of the type OverlayText."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'OverlayText' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-1-ee63c116d49f>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[0mtext\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mOverlayText\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      2\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      3\u001b[0m \u001b[0mtext\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mwidth\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;36m200\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      4\u001b[0m \u001b[0mtext\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mheight\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;36m400\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m \u001b[0mtext\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mleft\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;36m10\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mNameError\u001b[0m: name 'OverlayText' is not defined"
     ]
    }
   ],
   "source": [
    "text = OverlayText()\n",
    "\n",
    "text.width = 200\n",
    "text.height = 400\n",
    "text.left = 10\n",
    "text.top = 10\n",
    "text.text_size = 12\n",
    "text.line_width = 2\n",
    "text.font = \"DejaVu Sans Mono\"\n",
    "text.text = \"\"\"Distance=  %s.\n",
    "Angle=%s.\n",
    "Counter = <span style=\"color: green;\">%d.</span>\n",
    "\"\"\" % (str(number), str(number2) ,self.counter)\n",
    "text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)\n",
    "text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)\n",
    "\n",
    "text_pub.publish(text)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's talk about some of the variables of the class <b>OverlayText</b>:<br>\n",
    "<ul>\n",
    "<li>width, height, left, top: This states the dimensions of the Text area (width, height) and the position of the upper-left hand corner of the text (left, top).</li>\n",
    "<li>text_size, line_width, font: Font parameters.</li>\n",
    "<li>text: Here you can state the string you want, and bear in mind that you can update it all the time, giving dynamic text data.</li>\n",
    "<li>fg_color,bg_color: Colors of the background.</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have the complete message:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "[jsk_rviz_plugins/OverlayText]:                                                                                                                                           \n",
    "uint8 ADD=0                                                                                                                                                               \n",
    "uint8 DELETE=1                                                                                                                                                            \n",
    "uint8 action                                                                                                                                                              \n",
    "int32 width                                                                                                                                                               \n",
    "int32 height                                                                                                                                                              \n",
    "int32 left                                                                                                                                                                \n",
    "int32 top                                                                                                                                                                 \n",
    "std_msgs/ColorRGBA bg_color                                                                                                                                               \n",
    "  float32 r                                                                                                                                                               \n",
    "  float32 g                                                                                                                                                               \n",
    "  float32 b                                                                                                                                                               \n",
    "  float32 a                                                                                                                                                               \n",
    "int32 line_width                                                                                                                                                          \n",
    "float32 text_size                                                                                                                                                         \n",
    "string font                                                                                                                                                               \n",
    "std_msgs/ColorRGBA fg_color                                                                                                                                               \n",
    "  float32 r                                                                                                                                                               \n",
    "  float32 g                                                                                                                                                               \n",
    "  float32 b                                                                                                                                                               \n",
    "  float32 a                                                                                                                                                               \n",
    "string text  \n",
    "\"\"\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To visualise, add the element <b>OverlayText</b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Menu"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The menu is essentially a list with an index that indicates which element on the list is to be selected.<br>\n",
    "It's done like so:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "menu_publisher = rospy.Publisher(\"/test_menu\", OverlayMenu, queue_size=1)\n",
    "menu = OverlayMenu()\n",
    "menu.title = \"HaroSystemMode\"\n",
    "menu.menus = [\"Sleep\", \"Searching\", \"MovingInCircles\",\"Waiting\"]\n",
    "\n",
    "# Index 1 is Searching\n",
    "menu.current_index = 1\n",
    "menu_publisher.publish(menu)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You have a publisher like always, and you can change three things:<br>\n",
    "<ul>\n",
    "<li>title</li>\n",
    "<li>menus: It's a list with all the options.</li>\n",
    "<li>current_index: Option selected.</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You then have to add that element in RVIZ through the <b>OverlayMenu</b> element."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Complete demo"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have the program that generated all the overlays for the demo as an example. It essentially retrieves the linear distance and angles from HaroRobot to StandingPerson from the topic <b>/haro_base_link_to_person_standing_tf_translation</b>. Bear in mind that although it states translation, the data given is a twist. That is because it's simplified to give you the linear distance in the linear.x value directly, and the angle orientation through the angular.z variable:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">haro_overlay_complete_demo.py</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "from jsk_rviz_plugins.msg import OverlayText, OverlayMenu\n",
    "from std_msgs.msg import ColorRGBA, Float32\n",
    "import rospy\n",
    "import math\n",
    "import random\n",
    "from geometry_msgs.msg import Twist\n",
    "\n",
    "class HaroOverlay(object):\n",
    "    def __init__(self):\n",
    "        \n",
    "        self.text_pub = rospy.Publisher(\"/text_sample\", OverlayText, queue_size=1)\n",
    "        \n",
    "        self.plot_value_pub = rospy.Publisher(\"/plot_value_sample\", Float32, queue_size=1)\n",
    "        self.piechart_value_pub = rospy.Publisher(\"/piechart_value_sample\", Float32, queue_size=1)\n",
    "        self.menu_publisher = rospy.Publisher(\"/test_menu\", OverlayMenu, queue_size=1)\n",
    "        self.plot_value = 0\n",
    "        self.piechart_value = 0\n",
    "        self.max_distance_from_object = 10.0\n",
    "        \n",
    "        self.subs = rospy.Subscriber(\"/haro_base_link_to_person_standing_tf_translation\", Twist, self.twist_callback)\n",
    "        \n",
    "        self.counter = 0\n",
    "        self.rate = rospy.Rate(100)\n",
    "        self.overlaytext = self.update_overlaytext()\n",
    "        self.menu = self.update_overlay_menu()\n",
    "        \n",
    "        \n",
    "\n",
    "    def twist_callback(self, msg):\n",
    "        self.plot_value = msg.linear.x\n",
    "        self.piechart_value = msg.angular.z\n",
    "\n",
    "    def update_overlaytext(self, number=1.23, number2=20):  \n",
    "        \n",
    "        \n",
    "        \n",
    "        text = OverlayText()\n",
    "\n",
    "        text.width = 200\n",
    "        text.height = 400\n",
    "        text.left = 10\n",
    "        text.top = 10\n",
    "        text.text_size = 12\n",
    "        text.line_width = 2\n",
    "        text.font = \"DejaVu Sans Mono\"\n",
    "        text.text = \"\"\"Distance=  %s.\n",
    "        Angle=%s.\n",
    "        Counter = <span style=\"color: green;\">%d.</span>\n",
    "        \"\"\" % (str(number), str(number2) ,self.counter)\n",
    "        text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)\n",
    "        text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)\n",
    "        \n",
    "        return text\n",
    "    \n",
    "    def update_overlay_textonly(self, new_text):\n",
    "        self.overlaytext.text = new_text\n",
    "\n",
    "\n",
    "    def update_overlay_menu(self):\n",
    "        menu = OverlayMenu()\n",
    "        menu.title = \"HaroSystemMode\"\n",
    "        menu.menus = [\"Sleep\", \"Searching\", \"MovingInCircles\",\"Waiting\"]\n",
    "        \n",
    "        menu.current_index = self.counter % len(menu.menus)\n",
    "        return menu\n",
    "        \n",
    "    def update_overlay_menu_haro_tf(self):\n",
    "        menu = OverlayMenu()\n",
    "        menu.title = \"HaroDistanceFromPerson\"\n",
    "        menu.menus = [\"FarAway\", \"CloseBy\", \"Target\", \"OtherWayRound\"]\n",
    "        \n",
    "        fraction = 10.0\n",
    "        \n",
    "        if self.piechart_value < (math.pi/fraction):\n",
    "            if self.plot_value >= self.max_distance_from_object:\n",
    "                index = 0\n",
    "            elif self.plot_value >= (self.max_distance_from_object/ fraction) and self.plot_value < self.max_distance_from_object:\n",
    "                index = 1\n",
    "            elif self.plot_value < (self.max_distance_from_object/fraction):\n",
    "                index = 2\n",
    "            \n",
    "        else:\n",
    "            index = 3\n",
    "            \n",
    "        menu.current_index = index\n",
    "            \n",
    "        return menu\n",
    "\n",
    "    def start_dummy_demo(self):\n",
    "        \n",
    "        while not rospy.is_shutdown():\n",
    "            self.overlaytext = self.update_overlaytext()\n",
    "            self.menu = self.update_overlay_menu()\n",
    "            \n",
    "            if self.counter % 100 == 0:\n",
    "                self.menu.action = OverlayMenu.ACTION_CLOSE\n",
    "            \n",
    "            self.text_pub.publish(self.overlaytext)\n",
    "            # If values are very high it autoadjusts so be careful\n",
    "            self.value_pub.publish(math.cos(self.counter * math.pi * 2 / 1000))\n",
    "            self.menu_publisher.publish(self.menu)\n",
    "            \n",
    "            self.rate.sleep()\n",
    "            self.counter += 1\n",
    "    \n",
    "    def start_harodistance_demo(self):\n",
    "        \n",
    "        while not rospy.is_shutdown():\n",
    "            self.overlaytext = self.update_overlaytext(number=self.plot_value, number2=self.piechart_value)\n",
    "            self.menu = self.update_overlay_menu_haro_tf()\n",
    "            \n",
    "            self.text_pub.publish(self.overlaytext)\n",
    "            \n",
    "            self.plot_value_pub.publish(self.plot_value)\n",
    "            self.piechart_value_pub.publish(self.piechart_value)\n",
    "            \n",
    "            self.menu_publisher.publish(self.menu)\n",
    "            \n",
    "            self.rate.sleep()\n",
    "            self.counter += 1\n",
    "  \n",
    "\n",
    "\n",
    "  \n",
    "def dummy_overlay_demo():\n",
    "    rospy.init_node('distance_overlay_demo_node', anonymous=True)\n",
    "    haro_overlay_object = HaroOverlay()\n",
    "    try:\n",
    "        #haro_overlay_object.start_dummy_demo()\n",
    "        haro_overlay_object.start_harodistance_demo()\n",
    "    except rospy.ROSInterruptException:\n",
    "        pass\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    dummy_overlay_demo()\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END haro_overlay_complete_demo.py</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U3-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add the haro_overlay_complete_demo.py to your package and execute it. See what it gives and play around with the options of the different elements.<br>\n",
    "Then, do as an EXTRA:\n",
    "<ul>\n",
    "<li>Plot the x,y, and z positions of HaroRobot. Adjust the maximum and minimum values so that the graph shows significant data. Remove the AutoScale for this.</li>\n",
    "<li>Change the limits in the PieChart so that it changes color based on the values. Select the autocolor change to have that effect.</li>\n",
    "<li>Create a menu that changes based on a value in a topic. That way, anyone can change that state.</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U3-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see the basic script like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/rvizmarkers_unit3_demo.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations, you now know how to put Overlays in RVIZ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we will see how to use panels to turn RVIZ into your robot's GUI command system and give it some interactivity"
   ]
  }
 ],
 "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.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}