Course_RvizMarkers_Unit_4.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 4: Add Custom Panels to RVIZ and Extras"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Robotics is tough. Sometimes it's too Spartan, with only command executed programs. It always helps to have a simple GUI to execute the most used programs and give some fast interactivity.<br>\n",
    "This is what you are going to learn here:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Add a yes or no Interactive panel"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This panel will return if you click yes or no on the panel. This is very useful for confirmation of actions, such as: Can I proceed with making the grasp?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step is to add the panel to the RVIZ. For that, you only have to go to <b>Panels</b> and <b>AddPanel</b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"width:100%\">\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "      <img id=\"fig-A.1\" src=\"img/rvizmarkers_unit4_addpanel.png\" width=\"150\"/>       \n",
    "    </figure>\n",
    "\n",
    "    </th>\n",
    "    <th>\n",
    "        <figure>\n",
    "      <img id=\"fig-A.2\" src=\"img/rvizmarkers_unit4_addpanelyes.png\" width=\"300\"/>\n",
    "    </figure>\n",
    "    </th>\n",
    "\n",
    "  </tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, you have to create a program that calls a certain service and waits for the response. Here you have an example:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">panels_demo.py</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "import sys\n",
    "import rospy\n",
    "from jsk_gui_msgs.srv import YesNo, YesNoRequest, YesNoResponse\n",
    "\n",
    "\n",
    "class RvizPanels(object):\n",
    "    def __init__(self):\n",
    "        self.yes_or_no_service_name = '/rviz/yes_no_button'\n",
    "        rospy.wait_for_service(self.yes_or_no_service_name)\n",
    "        rospy.loginfo(self.yes_or_no_service_name+\"...Ready\")\n",
    "        \n",
    "    def ask_yes_or_no_rviz(self):\n",
    "    \n",
    "        try:\n",
    "            ask_yes_or_no_proxy = rospy.ServiceProxy(self.yes_or_no_service_name, YesNo)\n",
    "            yes_no_req = YesNoRequest()\n",
    "            responce = ask_yes_or_no_proxy(yes_no_req)\n",
    "            return responce.yes\n",
    "        except rospy.ServiceException, e:\n",
    "            print \"Service call failed: %s\"%e\n",
    "\n",
    "\n",
    "        \n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('demorvizpanels_node', anonymous=True)\n",
    "    rvizpanels_object = RvizPanels()\n",
    "    rospy.loginfo(\"Waiting for Answer in RVIZ panel\")\n",
    "    yes_answer = rvizpanels_object.ask_yes_or_no_rviz()\n",
    "    rospy.loginfo(\"Answer Yes=\"+str(yes_answer))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END panels_demo.py</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The service is always called <b>/rviz/yes_no_button</b>. It returns a service message of the type <b>YesNoResponse</b>. Here you have its structure:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "[jsk_gui_msgs/YesNo]:                                                                                                                                                     \n",
    "---                                                                                                                                                                       \n",
    "bool yes "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, the request is Empty, and the response is a boolean; true if YES was clicked."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U4-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a script that asks yes or no. If clicked, YES moves HaroRobot up and down. If not, then it moves it in circles."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U4-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Add a custom GUI"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, yes or no interactivity is ok, but it quickly gets smaller for robotics applications. For that, you have the full panel experience."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You have to add the panel <b>RobotCommandInterfaceAction</b>:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"width:100%\">\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "      <img id=\"fig-A.1\" src=\"img/rvizmarkers_unit4_addpanel.png\" width=\"150\"/>       \n",
    "    </figure>\n",
    "\n",
    "    </th>\n",
    "    <th>\n",
    "        <figure>\n",
    "      <img id=\"fig-A.2\" src=\"img/rviz_marker_unit4_addcompletepanel.png\" width=\"300\"/>\n",
    "    </figure>\n",
    "    </th>\n",
    "\n",
    "  </tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This starts with a config file like this one, <b>haro_robot_command.yaml</b>:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "robot_command_buttons:\n",
    "  - name: \"Execute Program\"\n",
    "    icon: \"package://haro_description/img/myimage.jpg\"\n",
    "    type: euscommand\n",
    "    command: \"move_in_circles\"\n",
    "  - name: \"Send EmptySrv message\"\n",
    "    icon: \"package://haro_description/img/myimage2.png\"\n",
    "    type: emptysrv\n",
    "    srv: \"/move_up_and_down\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can use these interactive panels in two ways:<br>\n",
    "<ul>\n",
    "<li>Using EmptySrv messages: This will send an emptysrv.srv message to the service stated in the <b>srv</b> tag. It's useful if only the activation of something is needed.</li>\n",
    "<li>Using euscommand type messages: This is used when you want to send data to the executing system. You have to use a service that HAS to be named <b>euscommand</b>, and of the EusCommand.srv type. It then allows you to send any information in string format through the field <b>command</b>.</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In both, you can state which icon is to be placed, retrieving it from a package. You can also state the name that will appear under the button."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a server that can process the EusCommand messages. It has to be like this one:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "service_server_name = \"/eus_command\"\n",
    "s = rospy.Service(service_server_name, EusCommand, handle_command)\n",
    "\n",
    "handle_command(req):\n",
    "    \"\"\"\n",
    "    [jsk_rviz_plugins/EusCommand.srv]\n",
    "    string command                                                                                                                 \n",
    "    ---  \n",
    "    \"\"\"\n",
    "    rospy.loginfo(\"Command Requested==>\"+str(req.command))\n",
    "    if req.command == \"move_in_circles\":\n",
    "        self.haro_object.move_for_x_time(num=20)\n",
    "    return EusCommandResponse()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this case with this service, if the string given is <b>move_in_circles</b>, it will move Haro in circles."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a launch file that launches the server and loads the <b>haro_robot_command.yaml<b>."
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "<launch>\n",
    "  <node pkg=\"rviz\" type=\"rviz\" name=\"$(anon rviz)\"\n",
    "        args=\"-d $(find my_rviz_markers)/rviz_config/haro_panels.rviz\">\n",
    "    <rosparam command=\"load\" file=\"$(find my_rviz_markers)/config/haro_robot_command.yaml\" />\n",
    "  </node>\n",
    "  \n",
    "  <node pkg=\"my_rviz_markers\" type=\"robot_command_server.py\" name=\"robot_command_server\"  output=\"screen\">\n",
    "  </node>\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This will launch RVIZ with a session of RVIZ that should have the panel activated. It will then launch the server that will listen to the buttons selected."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">robot_command_server.py</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from jsk_rviz_plugins.srv import EusCommand, EusCommandRequest, EusCommandResponse \n",
    "from std_srvs.srv import Empty, EmptyRequest, EmptyResponse\n",
    "from move_haro_in_circles import HaroMove\n",
    "\n",
    "class RobotCommandServer(object):\n",
    "    def __init__(self):\n",
    "        # To make the robot command RVIZ panel work it has to be this name\n",
    "        service_server_name = \"/eus_command\"\n",
    "        s = rospy.Service(service_server_name, EusCommand, self.handle_command)\n",
    "        rospy.loginfo(\"Service =>\"+str(service_server_name)+\",Ready to recieve commands...\")\n",
    "        \n",
    "        updown_service_server_name = \"/move_up_and_down\"\n",
    "        s = rospy.Service(updown_service_server_name, Empty, self.handle_updown_command)\n",
    "        rospy.loginfo(\"Service =>\"+str(updown_service_server_name)+\",Ready to recieve commands...\")\n",
    "        \n",
    "        self.haro_object = HaroMove()\n",
    "        \n",
    "\n",
    "    def handle_command(self,req):\n",
    "        \"\"\"\n",
    "        [jsk_rviz_plugins/EusCommand.srv]\n",
    "        string command                                                                                                                 \n",
    "        ---  \n",
    "        \"\"\"\n",
    "        rospy.loginfo(\"Command Requested==>\"+str(req.command))\n",
    "        if req.command == \"move_in_circles\":\n",
    "            self.haro_object.move_for_x_time(num=20)\n",
    "        return EusCommandResponse()\n",
    "        \n",
    "    def handle_updown_command(self,req):\n",
    "        \"\"\"\n",
    "        [std_srvs/Empty.srv]\n",
    "        ---  \n",
    "        \"\"\"\n",
    "        rospy.loginfo(\"Move Up And Down\")\n",
    "        self.haro_object.move_for_x_time_updown(num=100)\n",
    "        return EmptyResponse()\n",
    "    \n",
    "    def start_server_spin(self):\n",
    "        rospy.spin()\n",
    "\n",
    "\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('robot_command_server')\n",
    "    robotserver_object = RobotCommandServer()\n",
    "    robotserver_object.start_server_spin()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END robot_command_server.py</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of the python module that makes Haro move in circles, and up and down."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">move_haro_in_circles.py</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "from geometry_msgs.msg import Twist\n",
    "\n",
    "class HaroMove():\n",
    "    \n",
    "    def __init__(self):\n",
    "        self.haro_vel = rospy.Publisher('/haro/cmd_vel', Twist, queue_size=1)\n",
    "        self.ctrl_c = False\n",
    "        rospy.on_shutdown(self.shutdownhook)\n",
    "        self.rate = rospy.Rate(10) # 10hz\n",
    "    \n",
    "    def shutdownhook(self):\n",
    "            # works better than the rospy.is_shut_down()\n",
    "            print \"shutdown time! Stop the robot\"\n",
    "            cmd = Twist()\n",
    "            cmd.linear.x = 0.0\n",
    "            cmd.angular.z = 0.0\n",
    "            self.haro_vel.publish(cmd)\n",
    "            self.ctrl_c = True\n",
    "\n",
    "    def start(self):\n",
    "        cmd = Twist()\n",
    "        cmd.linear.x = 1.0\n",
    "        cmd.angular.z = 1.0\n",
    "        while not self.ctrl_c:\n",
    "            self.haro_vel.publish(cmd)\n",
    "            self.rate.sleep()\n",
    "    \n",
    "    def move_for_x_time(self, num=20):\n",
    "        cmd = Twist()\n",
    "        cmd.linear.x = 1.0\n",
    "        cmd.angular.z = 1.0\n",
    "        i = 0\n",
    "        while i <= num:\n",
    "            rospy.loginfo(i)\n",
    "            self.haro_vel.publish(cmd)\n",
    "            self.rate.sleep()\n",
    "            i += 1\n",
    "        self.shutdownhook()\n",
    "            \n",
    "    def move_for_x_time_updown(self, num=20):\n",
    "        cmd = Twist()\n",
    "        i = 0\n",
    "        while i <= num:\n",
    "            if i > num/2.0:\n",
    "                cmd.linear.z = -0.5\n",
    "                rospy.loginfo(\"Move Down\")\n",
    "            else:\n",
    "                cmd.linear.z = 0.5\n",
    "                rospy.loginfo(\"Move Up\")\n",
    "            self.haro_vel.publish(cmd)\n",
    "            self.rate.sleep()\n",
    "            i += 1\n",
    "        self.shutdownhook()\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    rospy.init_node('move_haro_in_circles_node', anonymous=True)\n",
    "    haro_object = HaroMove()\n",
    "    try:\n",
    "        haro_object.move_for_x_time(num=20)\n",
    "    except rospy.ROSInterruptException:\n",
    "        pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END move_haro_in_circles.py</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/rvizmarkers_unit4_custompanel.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U4-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create your own Custom Panel that has the following features:<br>\n",
    "<ul>\n",
    "<li>Has Three Buttons</li>\n",
    "<li>You can make Haro turn itself on</li>\n",
    "<li>You can make the StandingPerson move in circles</li>\n",
    "<li>You can change the pressing of one of the buttons of the Pictogram on top of Haro</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U4-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Draw Trajectories and TwistStamped in RVIZ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is super simple. Just add the following elements and you will have represented the TF trajectories and the TwistStamped sent to move Haro.<br>\n",
    "You have to add the <b>TFTrajectory</b> and select the frame you want to draw the trajectory.<br>\n",
    "You have to add the <b>TwistStamped</b> and select the topic of the type TwistStamped. In this case, it is <b>rostopic echo /haro/twist_stamped_publisher</b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/rvizmarkers_unit4_addtraj.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should then see something similar to this when you move Haro through the command:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p><br>\n",
    "roslaunch haro_description move_haro_keyboard.launch\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you press the keys continuously, the TwistStamped arrows will stay there until you stop pressing."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/rvizmarkers_unit4_stamptraj.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Record RVIZ Sessions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add the <b>VideoCapture</b> element to be able to record videos by just checking the <b>start_capture box</b>.<br>\n",
    "To stop the video, just uncheck the <b>start_capture box</b>. It's that simple."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/rvizmarkers_unit4_record.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:blue;color:white;\">You have to state the <b>filename,</b> which is the path to record to. Be careful because these files grow very quickly in size.</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations! You have finished the RVIZ Markers course!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What's next? Continue your ROS learning and Robotics projects with us."
   ]
  }
 ],
 "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
}