Course_FlexBe_Unit1.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Using FlexBe with ROS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_img.png\" width=\"1000\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 1: Create a basic behavior"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time to completion: <b>2 hours</b><br><br>\n",
    "In this unit, you are going to learn the basic concepts of FlexBe, and you are going to review, step by step, how to create a basic behavior and a basic state."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before beginning work with FlexBe, let's have a look at some basic concepts you need to know."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "FlexBe allows you to create complex robot behaviors without the need of manually coding them. Based on basic capabilities, which interface standard functionality, you can easily compose state machines using the drag and drop editor that FlexBe provides in its GUI. These basic capabilities are also known as **states**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "States define what should be done by a behavior. They are grouped as state machines, defining the control flow when the behavior is executed. Therefore, each state declares a set of outcomes, which represent possible results of execution. Furthermore, data can be shared and modified during runtime between states. This data is called userdata and is stored as key-value-pairs. Each state declares which keys it requires (input) and provides (output).\n",
    "\n",
    "**FlexBE states** are the high-level building blocks from which behaviors are constructed and are supposed to interface with the capabilities of your robot system.\n",
    "\n",
    "Typically, implementing states is the only time when you will manually write code while working with FlexBE. In the following exercise, you will create your first FlexBe state."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Creating a new state"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 1.1**</p>\n",
    "<br>\n",
    "\n",
    "a) Go to your **catkin_ws/src** folder and create a new FlexBe environment. You can do it by issuing the following commands."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cd caktin_ws/src"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun flexbe_widget create_repo turtlebot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If everything went OK, a new structure of folders and files will have appeared in your workspace, under the name of **turtlebot_behaviors**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/turtlebot_behavior_structure.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Within this **turtlebot_behaviors** folder, you have all the structure needed to create a new FlexBe behavior, alongside some example files. If you navigate into **\"turtlebot_behaviors/turtlebot_flexbe_states/src/turtlebot_flexbe_states\"**, you will find a couple of example scripts for FlexBe states. You can have a look at them, if you wish, since they are very well commented. Inside this folder is where we will place our new code."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Whenever you create a new behavior, it will be necessary to compile your workspace. So, let's do that now."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cd .."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_make"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "source devel/setup.bash"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Let's add our own state. For that, let's create a new script inside **\"turtlebot_behaviors/turtlebot_flexbe_states/src/turtlebot_flexbe_states\"**, which we can call **drive_forward.py**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cd src/turtlebot_behaviors/turtlebot_flexbe_states/src/turtlebot_flexbe_states/"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "touch drive_forward.py;chmod +x drive_forward.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside this new script, copy the following content."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**drive_forward.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "from flexbe_core import EventState, Logger\n",
    "from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached\n",
    "from geometry_msgs.msg import Twist\n",
    "from sensor_msgs.msg import LaserScan\n",
    "\n",
    "class GoFowardState(EventState):\n",
    "    '''\n",
    "\tDriving state for a ground robot. This state allows the robot to drive forward a certain distance\n",
    "    at a specified velocity/ speed.\n",
    "\n",
    "\t-- speed \tfloat \tSpeed at which to drive the robot\n",
    "    -- travel_dist float   How far to drive the robot before leaving this state\n",
    "    -- obstacle_dist float Distance at which to determine blockage of robot path\n",
    "\n",
    "\t<= failed \t\t\t    If behavior is unable to ready on time\n",
    "\t<= done \t\t\t\tExample for a failure outcome.\n",
    "\n",
    "\t'''\n",
    "    def __init__(self, speed, travel_dist, obstacle_dist):\n",
    "        super(GoFowardState, self).__init__(outcomes=['failed', 'done'])\n",
    "        self._start_time = None\n",
    "        self.data = None\n",
    "        self._speed = speed\n",
    "        self._travel_dist = travel_dist\n",
    "        self._obstacle_dist = obstacle_dist\n",
    "\n",
    "        self.vel_topic = '/cmd_vel'\n",
    "        self.scan_topic = '/kobuki/laser/scan'\n",
    "\n",
    "        #create publisher passing it the vel_topic_name and msg_type\n",
    "        self.pub = ProxyPublisher({self.vel_topic: Twist})\n",
    "        #create subscriber\n",
    "        self.scan_sub = ProxySubscriberCached({self.scan_topic: LaserScan})\n",
    "        self.scan_sub.set_callback(self.scan_topic, self.scan_callback)\n",
    "    \n",
    "    def execute(self, userdata):\n",
    "        if not self.cmd_pub:\n",
    "            return 'failed'\n",
    "        #run obstacle checks [index 0: left, 360: middle, 719: right]\n",
    "        if(self.data is not None):\n",
    "            Logger.loginfo('FWD obstacle distance is: %s' % self.data.ranges[360])\n",
    "            if self.data.ranges[360] <= self._obstacle_dist:\n",
    "                return 'failed'\n",
    "    \n",
    "            #measure distance travelled\n",
    "            elapsed_time = (rospy.Time.now() - self._start_time).to_sec()\n",
    "            distance_travelled = (elapsed_time/ 60.0) * self._speed\n",
    "    \n",
    "            if distance_travelled >= self._travel_dist:\n",
    "                return 'done'\n",
    "        \n",
    "        #drive\n",
    "        self.pub.publish(self.vel_topic, self.cmd_pub)\n",
    "\n",
    "    def on_enter(self, userdata):\n",
    "        Logger.loginfo(\"Drive FWD STARTED!\")\n",
    "        #set robot speed here\n",
    "        self.cmd_pub = Twist()\n",
    "        self.cmd_pub.linear.x = self._speed\n",
    "        self.cmd_pub.angular.z = 0.0\n",
    "        \n",
    "    def on_exit(self, userdata):\n",
    "        self.cmd_pub.linear.x = 0.0\n",
    "        self.pub.publish(self.vel_topic, self.cmd_pub)\n",
    "        self.scan_sub.unsubscribe_topic(self.scan_topic)\n",
    "        Logger.loginfo(\"Drive FWD ENDED!\")\n",
    "        \n",
    "    def on_start(self):\n",
    "        Logger.loginfo(\"Drive FWD READY!\")\n",
    "        self._start_time = rospy.Time.now() #bug detected! (move to on_enter)\n",
    "        \n",
    "    def on_stop(self):\n",
    "\t\tLogger.loginfo(\"Drive FWD STOPPED!\")\n",
    "\n",
    "    def scan_callback(self, data):\n",
    "        self.data = data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**drive_forward.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Don't worry if you don't understand the whole code right now. We'll review and explain everything in detail in this chapter. For now, all you need to know is that the purpose of the above state is to allow a robot to drive forward a certain distance at a specified velocity/speed."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Compile again your workspace."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roscd;cd .."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_make"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "source devel/setup.bash"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 1.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, basically, a state defines a basic functionality. In FlexBe, we call any class with the following form a state:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class ClassNameState(EventState):\n",
    "    def __init__(self):\n",
    "        super(ClassNameState, self).__init__(outcomes=['done', 'fail'])\n",
    "        \n",
    "    def execute(self, userdata):\n",
    "        pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, from the code snippet above, we can tell three main things from FlexBe states:\n",
    "\n",
    "- They inherit from the **EventState** class.\n",
    "- They return an outcome in the form of: **done** or **fail**.\n",
    "- They implement the **execute()** function."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But, as you may have noticed from the script you saw in Exercise 1.1, there are some more things involved. And you are totally right! We also have the state **events**. Currently, six implementable events exist: \n",
    "\n",
    "- **on_start**: This event is triggered when the **behavior** is started. *It is recommended, if possible, to initialize used resources in the constructor.*\n",
    "\n",
    "\n",
    "- **on_enter**: This event is triggered when a **state** becomes active, i.e., when a transition occurs. *Primarily used to start actions that are associated with this state or initialize variables that may have changed during a previous execution.*\n",
    "\n",
    "\n",
    "- **on_exit**: This event is triggered when an **outcome** is returned and another state becomes active. *It can be used to stop running processes that were started by on_enter.*\n",
    "\n",
    "\n",
    "- **on_stop**: This event is triggered whenever the **behavior** stops execution, and also if it is cancelled. *Used for cleaning up claimed resources or running processes even if the state is not active.*\n",
    "\n",
    "\n",
    "- **on_pause** (similar to on_exit): This event is triggered when:\n",
    "    - the operator clicks on the Pause button in the user interface \n",
    "    or \n",
    "    - execution is paused automatically, e.g., because of a priority container becoming active. *It is recommended to implement this event similar to on_exit.*\n",
    "\n",
    "\n",
    "- **on_resume**  (similar to on_enter): Complement to on_pause, i.e., this event is triggered when execution resumes after having been paused. *It is recommended to implement this event similar to on_enter.*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With the proper concepts presented, let's have a look at the following image."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/state_lifecycle.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The above image shows the lifecycle of a state, along with associated events. When a new behavior is executed, this lifecycle is started for all states contained anywhere in the hierarchy of the behavior. Basically, the process goes as follows:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. First, all states are instantiated and thus, their constructor is called (**\\__init __()**). \n",
    "\n",
    "2. Afterwards, i.e., when the execution of the constructor of all states has finished, the **on_start** event is triggered for all states. \n",
    "\n",
    "3. Then, the initial state becomes active. Whenever a state becomes active, its **on_enter** event is triggered once. \n",
    "\n",
    "4. Then, it periodically (default: 10 hz) calls its **execute** function to check for exit conditions. \n",
    "\n",
    "5. If an outcome is returned, the **on_exit** event is triggered once before the next state becomes active. \n",
    "\n",
    "6. Finally, if an outcome of the root-level state machine is returned or execution of the behavior is stopped manually, the **on_stop** event of all states is triggered in order to clean up."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that **on_start** and **on_stop** are triggered exactly once for each state (concurrently) per behavior execution. Although **on_enter** and **on_exit** are also only triggered once when the associated state becomes active or inactive, a state can become active multiple times and, thus, these events can be triggered multiple times during a behavior execution."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So now that we already know how the whole structure and lifecycle of a state works, let's have a closer look at the state that we created in the previous exercise, 1.1."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "from flexbe_core import EventState, Logger\n",
    "from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached\n",
    "from geometry_msgs.msg import Twist\n",
    "from sensor_msgs.msg import LaserScan"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, we can see some imports that we will need for this class. From those imports, we can highlight the **EventState** and **Logger** classes, from the **flexbe_core** module. The EventState class is the basic state, from where all other states will inherit. The Logger class provides the FlexBe logging functionality.\n",
    "\n",
    "Also, pay atenttion to the **ProxyPublisher** and **ProxySubscriberCached** classes, from **flexbe_core.proxy**. FlexBe provides a set of **proxies**, which are more efficient implementations of basic ROS elements, like Publishers or Subscribers. We'll see more on them right now."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def __init__(self, speed, travel_dist, obstacle_dist):\n",
    "    super(GoFowardState, self).__init__(outcomes=['failed', 'done'])\n",
    "    self._start_time = None\n",
    "    self.data = None\n",
    "    self._speed = speed\n",
    "    self._travel_dist = travel_dist\n",
    "    self._obstacle_dist = obstacle_dist\n",
    "\n",
    "    self.vel_topic = '/cmd_vel'\n",
    "    self.scan_topic = '/kobuki/laser/scan'\n",
    "\n",
    "    #create publisher passing it the vel_topic_name and msg_type\n",
    "    self.pub = ProxyPublisher({self.vel_topic: Twist})\n",
    "    #create subscriber\n",
    "    self.scan_sub = ProxySubscriberCached({self.scan_topic: LaserScan})\n",
    "    self.scan_sub.set_callback(self.scan_topic, self.scan_callback)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we have the constructor of the class. As you can see, it takes as input of three parameters: **speed**, **travel_dist**, and **obstacle_dist**. These are the speed we want our robot to move at, the distance we want our robot to travel, and the maximum distance to an obstacle before the robot stops (to avoid a potential crash)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we are initializing some variables we'll use on the class, including the inputs we've just talked about, which we also store in variables of the class."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, we are storing the topics we are going to use in variables. In the **/cmd_vel** topic, we will publish in order to move the robot. And we are going to subscribe to the **/kobuki/laser/scan** topic to get the readings from the laser mounted on the robot (in order to get the distance to obstacles). It is always recommended to store the topics in variables, since you will need them again later."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we define our proxies for publishing and for subscribing to the topics. So, we are using the **ProxyPublisher** and the **ProxySubscriberCached**. Four types of proxies exist in FlexBe:\n",
    "\n",
    "* **ProxyPublisher**: It is the equivalent to the ROS Publisher.\n",
    "* **ProxySubscriberCached**: It is the equivalent to the ROS Subscribers.\n",
    "* **ProxyServiceCaller**: It is the equivalent to the ROS Service Client.\n",
    "* **ProxyActionClient**: It is the equivalent to the ROS Action Client."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "All the proxies are instantiated by passing them a dictionary of topics and their types to work with. For instance:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.pub = ProxyPublisher({'/a_topic': MsgType})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And you can pass as many topics as you want to it, just like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.pub = ProxyPublisher({'/a_topic': MsgType, '/another_topic': MsgType, ...})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we are also setting the callback function for the laser subscriber, using the **set_callback()** function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def execute(self, userdata):\n",
    "        if not self.cmd_pub:\n",
    "            return 'failed'\n",
    "        #run obstacle checks [index 0: left, 360: middle, 719: right]\n",
    "        if(self.data is not None):\n",
    "            Logger.loginfo('FWD obstacle distance is: %s' % self.data.ranges[360])\n",
    "            if self.data.ranges[360] <= self._obstacle_dist:\n",
    "                return 'failed'\n",
    "    \n",
    "            #measure distance travelled\n",
    "            elapsed_time = (rospy.Time.now() - self._start_time).to_sec()\n",
    "            distance_travelled = (elapsed_time/ 60.0) * self._speed\n",
    "    \n",
    "            if distance_travelled >= self._travel_dist:\n",
    "                return 'done'\n",
    "        \n",
    "        #drive\n",
    "        self.pub.publish(self.vel_topic, self.cmd_pub)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next we have the **execute()** function. Inside this function, we are checking several things. The workflow goes as follows:\n",
    "\n",
    "1. First, we check if a message (in **self.cmd_pub**) to publish into the **/cmd_vel** topic exists. If not, then we set the state as **failed** and we exit.\n",
    "2. Second, we check the data related to the laser readings (which is stored in **self.data**). If the readings indicate that there is an obstacle closer to the robot than the **self._obstacle_dist**, we exit and set the state to **failed**. Also, we check if the current distance that the robot has traveled is higher or equal to **self._travel_dist**, and if it is, we exit and set the state to **done**.\n",
    "3. Finally, we publish into the **/cmd_vel** topic in order to move the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Summarizing, we poll the scan data to check for obstacles in the path. If there are any, we return an outcome of **failed**, if not, we keep driving forward the specified distance, and return an outcome of **done** when finished."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def on_enter(self, userdata):\n",
    "    Logger.loginfo(\"Drive FWD STARTED!\")\n",
    "    #set robot speed here\n",
    "    self.cmd_pub = Twist()\n",
    "    self.cmd_pub.linear.x = self._speed\n",
    "    self.cmd_pub.angular.z = 0.0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we have the **on_enter()** state. This event is called when this state is transitioned to. Here, we are just creating the message to be published by the *ProxyPublisher.*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def on_exit(self, userdata):\n",
    "    self.cmd_pub.linear.x = 0.0\n",
    "    self.pub.publish(self.vel_topic, self.cmd_pub)\n",
    "    self.scan_sub.unsubscribe_topic(self.scan_topic)\n",
    "    Logger.loginfo(\"Drive FWD ENDED!\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **on_exit()** event is called when leaving this state. Here is where we stop the robot from driving any further. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def on_start(self):\n",
    "    Logger.loginfo(\"Drive FWD READY!\")\n",
    "    self._start_time = rospy.Time.now() #bug detected! (move to on_enter)\n",
    "    \n",
    "def on_stop(self):\n",
    "    Logger.loginfo(\"Drive FWD STOPPED!\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the **on_start()** and **on_stop()** event implementations of this state, we are just setting the **self._start_time** value in the case of the former, and printing out some log messages."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def scan_callback(self, data):\n",
    "    self.data = data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, in our callback function for the laser subscriber, we are just saving the incoming laser data for later use in the variable of the **sel.data** class."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! So, at this point, you have just created your first FlexBe state. But as for being able to use this state for something, you'll need to include it into a behavior. So now, we are going to create a new behavior in FlexBe, and we are going to use our new state in it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Creating a new behavior"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, let's now try to use our new state in FlexBe! For that, you'll need to first launch the FlexBe application."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 1.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) To launch the FlexBe application, you can use the following 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>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch flexbe_app flexbe_full.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:red;\">**IMPORTANT NOTE:** If you launch FlexBe from different Web Shells, remember to always source the workspace before with the below command.</span>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "source ~/catkin_ws/devel/setup.bash"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:red;\">**If you don't source the workspace, you may encounter erros.**</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "Now, click the icon with a screen in the top-right corner of the IDE window \n",
    "<img src=\"img/font-awesome_desktop.png\"/> \n",
    "in order to open the Graphic Interface window. In the new window, the FlexBe app should have appeared."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_app.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) In the **Behavior Dashborad** window, you'll see several sections. First of all, let's fill in the **Overview** section. Take a look at the following caption and fill in yours."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_overview.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Now, let's move to the **Statemachine Editor** window. Here you will have the main Drag & Drop window."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the top section of this window, you'll see a button that is called **Add State**. When you click it, a new pop-up will appear at the right side of the screen."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_addstate.png\" width=\"200\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_addstate_panel1.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you can select the package where your new state is located, **turtlebot_flexbe_states**. Once you do that, you will have your state available for selection."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_addstate_panel2.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Select your state, give it a name, and click on the Add button. Automatically, a new block will appear in your drag & drop section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div style=\"text-align: left\">\n",
    "<img src=\"img/flexbe_addsatate_panel3.png\" width=\"400\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/flexbe_dragdrop1.png\" width=\"400\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Now, let's reorganize our blocks a little bit, and click on the **Auto-Connect** button at the top-right corner of the FlexBe App. You will see how, automatically, some connections are created."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div style=\"text-align: left\">\n",
    "<img src=\"img/flexbe_autoconnect.png\" width=\"300\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/flexbe_dragdrop2.png\" width=\"500\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Complete the connections of your state by connecting the **done** outcome to **finished**. It should look something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_dragdrop3.png\" width=\"700\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "f) The next step will be to set up the parameters, which are the inputs of your state. Remember that your state has three inputs: **speed**, **travel_dist**, and **obstacle_dist**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def __init__(self, speed, travel_dist, obstacle_dist):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Go back to the **Behavior Dashboard** and localize the **Behavior Paramters** section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_params2.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's first add a paramter called **my_speed**. Choose the **Numeric** type, set the default value to 0.4, and set the maximum and minimum values to 0 and 1, respectively. You can have a look at the image below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_params3.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Do the same for the other two parameters: travel distance and obstacle distance. You can have a look at the images below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<div style=\"text-align: left\">\n",
    "<img src=\"img/flexbe_turn_angle.png\" width=\"400\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "<img src=\"img/flexbe_turn_speed.png\" width=\"400\" style=\"float: left; margin: 0px 0px 15px 15px;\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have created all three parameters, let's go back to the **Statemachine editor**. Click on the block of your state and you should see a panel like this one in the right side of the window. Here you will need to connect all the inputs of your state to the parameters that you have just created."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_params5.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you select your parameters, you will see that they are automatically remapped to the variables of the class, in the form of **self.parameter**. In the end, you should have something like this."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_params6.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, remember to click on the **Apply** button to apply the recent changes. Also, click on the **Save Behavior** button. Saving the behavior is VERY IMPORTANT. If you don't, you won't be able to move the final step."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_save_behavior.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "g) The last step will be to actually run our behavior. So, first of all, let's move to the **Runtime Control** window. You will see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_runtime.png\" width=\"800\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you will be able to execute your behavior by clicking on the **Start Execution** button. Also, as you can see, you can modify the values of the parameters however you want. So, let's click on the **Start Execution** button and see our new behavior in action!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see the Turtlebot robot moving forwards, and stopping once it's too close to the wall."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<img src=\"img/flexbe1.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:red;\">**¡¡VERY IMPORTANT!!** Remember that, whenever you are done with the Exercise, you can reset the robot's position by hitting on the button **Reset the simulation!**, at the top-right corner of the simulation screen.</span>\n",
    "\n",
    "<img src=\"img/reset_sim.png\" width=\"300\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 1.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So at this point, you already know how to create a new FlexBe behavior, and also how to add a new state to it. Now, you could add as many more states as you wish in order to create more complex behaviors. In the following unit, we are going to add a new state to our behavior!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## Congratulations!! You are now capable of creating your own FlexBe behavior, and adding new states to it."
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [Root]",
   "language": "python",
   "name": "Python [Root]"
  },
  "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.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}