Course_FlexBe_Unit2.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 2: Action Client and Autonomy Levels"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time to completion: <b>3 hours</b><br><br>\n",
    "In this unit, you are going to learn how to create a new state based on an Action Client, and also how to use the Autonomy Levels in FlexBe."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in the previous unit, you learned how to create a new FlexBe state, and also how to add a new state to your behavior. At this point, what you have is a FlexBe behavior that allows a mobile robot (the Turtlebot, in this case) to travel a certain distance, taking into account the obstacles that may appear in its path."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But, of course, you can add as many more states as you wish in order to generate a more complex behavior. And that's exactly what we are going to do next. In the state you saw in the previous unit, you used two kinds of FlexBe proxies: the **ProxyPublisher** and the **ProxySubscriberCached**. In the new state we are going to add to our behavior, though, we are going to be using another kind of FlexBe proxy: the **ProxyActionClient**. These state are also known as **Actionlib states**, and are very common in FlexBe development."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **ProxyActionClient** provides an interface where robot capabilities can be provided by ROS nodes implementing an action server, and states can then access them by acting as an action client. The interface is designed for long-term (longer than one update cycle) actions, as it is non-blocking and optionally provides feedback while being executed. This makes it a perfect interface to be used along with FlexBe, and it is recommended to base your states on action interfaces whenever they do something more complex than just subscribing to a topic."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Creating an Actionlib state"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 2.1**</p>\n",
    "<br>\n",
    "\n",
    "a) First of all, let's add our new state. For that, let's create a new script inside **\"turtlebot_behaviors/turtlebot_flexbe_states/src/turtlebot_flexbe_states\"**, which we can call **turn_state.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 turn_state.py;chmod +x turn_state.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;\">**turn_state.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "import math\n",
    "from flexbe_core import EventState, Logger\n",
    "from flexbe_core.proxy import ProxyActionClient\n",
    "\n",
    "from spin_action_server.msg import SpinAction, SpinGoal, SpinResult, SpinFeedback\n",
    "\n",
    "class TurnState(EventState):\n",
    "    '''\n",
    "\tAvoidObstacleState implements a couple of basic obstacle avoidance algorithms. This state allows the robot to choose a different\n",
    "    to go to.\n",
    "\n",
    "    -- turn_angle   float   The angle that the robot should make\n",
    "\t-- t_speed \tfloat \tSpeed at which to turn the robot\n",
    "\n",
    "\t<= done \t\t    If turn completed successfully\n",
    "\n",
    "\t'''\n",
    "    def __init__(self, turn_angle, t_speed):\n",
    "        super(TurnState, self).__init__(outcomes=['done', 'failed'])\n",
    "        self._turn_angle = turn_angle\n",
    "        self._t_speed = t_speed\n",
    "        \n",
    "        self._topic = '/spin_server_X'\n",
    "        \n",
    "        #create the action client passing it the spin_topic_name and msg_type\n",
    "        self._client = ProxyActionClient({self._topic: SpinAction}) # pass required clients as dict (topic: type)\n",
    "        \n",
    "        # It may happen that the action client fails to send the action goal.\n",
    "        self._error = False\n",
    "    \n",
    "    def execute(self, userdata):\n",
    "        # While this state is active, check if the action has been finished and evaluate the result.\n",
    "        \n",
    "        # Check if the client failed to send the goal.\n",
    "        if self._error:\n",
    "            return 'failed'\n",
    "            \n",
    "        # Check if the action has been finished\n",
    "        if self._client.has_result(self._topic):\n",
    "            result = self._client.get_result(self._topic)\n",
    "            result_val = result.done\n",
    "            \n",
    "            # Based on the result, decide which outcome to trigger.\n",
    "            if result_val == \"done\":\n",
    "                return 'done'\n",
    "            else:\n",
    "                return 'failed'\n",
    "        \n",
    "        # Check if there is any feedback\n",
    "        if self._client.has_feedback(self._topic):\n",
    "            feedback = self._client.get_feedback(self._topic)\n",
    "            Logger.loginfo(\"Current heading is: %s\" % feedback.heading)\n",
    "            \n",
    "        # If the action has not yet finished, no outcome will be returned and the state stays active.\n",
    "\n",
    "\n",
    "    def on_enter(self, userdata):\n",
    "        # When entering this state, we send the action goal once to let the robot start its work.\n",
    "        # Create the goal.\n",
    "        goal = SpinGoal()\n",
    "        goal.angle = self._turn_angle\n",
    "        goal.turn_speed = self._t_speed\n",
    "        \n",
    "        # Send the goal.\n",
    "        self._error = False # make sure to reset the error state since a previous state execution might have failed\n",
    "        try:\n",
    "            self._client.send_goal(self._topic, goal)\n",
    "        except Exception as e:\n",
    "            # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.\n",
    "            # Using a linebreak before appending the error log enables the operator to collapse details in the GUI.\n",
    "            Logger.logwarn('Failed to send the Spin command:\\n%s' % str(e))\n",
    "            self._error = True\n",
    "        \n",
    "    def on_exit(self, userdata):\n",
    "        # Make sure that the action is not running when leaving this state.\n",
    "        # A situation where the action would still be active is for example when the operator manually triggers an outcome.\n",
    "        \n",
    "        if not self._client.has_result(self._topic):\n",
    "            self._client.cancel(self._topic)\n",
    "            Logger.loginfo('Cancelled active spin action goal.')\n",
    "        \n",
    "    def on_start(self):\n",
    "        Logger.loginfo(\"Robot Turn state: READY!\")\n",
    "        \n",
    "    def on_stop(self):\n",
    "        Logger.loginfo(\"Robot Turn: Disengaged!\")\n",
    "        \n",
    "        #same implementation as on exit\n",
    "        if not self._client.has_result(self._topic):\n",
    "            self._client.cancel(self._topic)\n",
    "            Logger.loginfo('Cancelled active spin action goal.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**turn_state.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can have a look at the code above to try to see what it will do. But don't worry if you don't understand the whole code right now. We'll review and explain everything in detail in this chapter."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 2.1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's now have a closer look at the state that we created in the previous exercise, 2.1."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "import math\n",
    "from flexbe_core import EventState, Logger\n",
    "from flexbe_core.proxy import ProxyActionClient\n",
    "\n",
    "from spin_action_server.msg import SpinAction, SpinGoal, SpinResult, SpinFeedback"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, we can see some imports that we will need for this class. Some of them are similar to the script we saw in the previous unit, like the **EventState** and **Logger** classes from the **flexbe_core** module.\n",
    "\n",
    "Also, pay attention to the **ProxyActionClient** class, from **flexbe_core.proxy**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def __init__(self, turn_angle, t_speed):\n",
    "    super(TurnState, self).__init__(outcomes=['done', 'failed'])\n",
    "    self._turn_angle = turn_angle\n",
    "    self._t_speed = t_speed\n",
    "\n",
    "    self._topic = '/spin_server_X'\n",
    "\n",
    "    #create the action client passing it the spin_topic_name and msg_type\n",
    "    self._client = ProxyActionClient({self._topic: SpinAction}) # pass required clients as dict (topic: type)\n",
    "\n",
    "    #It may happen that the action client fails to send the action goal.\n",
    "    self._error = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we have the constructor of the class. As you can see, it takes two parameters as input: **turn_angle** and **t_speed**. These are the angle at which we want our robot to turn and the maximum speed at which we want it done."
   ]
  },
  {
   "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 topic we are going to use in a variable. In this case, it's the topic of the Action Server we will connect to, which is **/spin_server_X**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we define the proxy to connect to the Action Server. So, we are using the **ProxyActionClient**. As you can see, this proxy is also instantiated by passing a dictionary with the topic name to it, and the message to use (**SpinAction**, in this case)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self._client = ProxyActionClient({self._topic: SpinAction})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we are initializing the error variable to False."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def execute(self, userdata):\n",
    "    # While this state is active, check if the action has been finished and evaluate the result.\n",
    "\n",
    "    # Check if the client failed to send the goal.\n",
    "    if self._error:\n",
    "        return 'failed'\n",
    "\n",
    "    # Check if the action has been finished\n",
    "    if self._client.has_result(self._topic):\n",
    "        result = self._client.get_result(self._topic)\n",
    "        result_val = result.done\n",
    "\n",
    "        # Based on the result, decide which outcome to trigger.\n",
    "        if result_val == \"done\":\n",
    "            return 'done'\n",
    "        else:\n",
    "            return 'failed'\n",
    "\n",
    "    # Check if there is any feedback\n",
    "    if self._client.has_feedback(self._topic):\n",
    "        feedback = self._client.get_feedback(self._topic)\n",
    "        Logger.loginfo(\"Current heading is: %s\" % feedback.heading)\n",
    "\n",
    "    # If the action has not yet finished, no outcome will be returned and the state stays active."
   ]
  },
  {
   "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 the value of the variable **self._error** is True. If it is, then we exit the function and return a **failed** state.\n",
    "2. Second, we check if the Action Server has returned a result yet (has finished). If it has returned a result, then we check the value of that result. If the value is **done**, then we exit and set the state to **done**. If it's not done, then we exit and set the state to **failed**.\n",
    "3. Finally, we check if there is any feedback from the Action Server. If there is, then we publish that feedback."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def on_enter(self, userdata):\n",
    "    # When entering this state, we send the action goal once to let the robot start its work.\n",
    "    # Create the goal.\n",
    "    goal = SpinGoal()\n",
    "    goal.angle = self._turn_angle\n",
    "    goal.turn_speed = self._t_speed\n",
    "\n",
    "    # Send the goal.\n",
    "    self._error = False # make sure to reset the error state since a previous state execution might have failed\n",
    "    try:\n",
    "        self._client.send_goal(self._topic, goal)\n",
    "    except Exception as e:\n",
    "        # Since a state failure does not necessarily cause a behavior failure, it is recommended to only print warnings, not errors.\n",
    "        # Using a linebreak before appending the error log enables the operator to collapse details in the GUI.\n",
    "        Logger.logwarn('Failed to send the Spin command:\\n%s' % str(e))\n",
    "        self._error = True"
   ]
  },
  {
   "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 creating the message (goal) to be sent to the Action Server. But note that we are also sending the goal here:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self._client.send_goal(self._topic, goal)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When working with Actionlib states, the **on_enter()** event is where you should create and also send the goal to the Action Server."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def on_exit(self, userdata):\n",
    "    # Make sure that the action is not running when leaving this state.\n",
    "    # A situation where the action would still be active is, for example, when the operator manually triggers an outcome.\n",
    "\n",
    "    if not self._client.has_result(self._topic):\n",
    "        self._client.cancel(self._topic)\n",
    "        Logger.loginfo('Cancelled active spin action goal.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **on_exit()** event is called when leaving this state. Here we are cancelling the goal, if it is still active. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def on_start(self):\n",
    "    Logger.loginfo(\"Robot Turn state: READY!\")\n",
    "\n",
    "def on_stop(self):\n",
    "    Logger.loginfo(\"Robot Turn: Disengaged!\")\n",
    "\n",
    "    #same implementation as on exit\n",
    "    if not self._client.has_result(self._topic):\n",
    "        self._client.cancel(self._topic)\n",
    "        Logger.loginfo('Cancelled active spin action goal.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the **on_start()** and **on_stop()** event implementations of this state, we are just printing out some log messages and cancelling the goal (again), if it is still active."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! So, at this point, you have created your first FlexBe Actionlib state. But before being able to use this state, you'll need something else, don't you think? Do you know what's missing here?\n",
    "\n",
    "Yes! That's right! The Action Server is missing! So, the next step will be to create our Action Server."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 2.2**</p>\n",
    "<br>\n",
    "\n",
    "a) First of all, let's create a new package where we will put all the files and code required by our Action Server."
   ]
  },
  {
   "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 ../src"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_create_pkg spin_action_server rospy actionlib actionlib_msgs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Inside the **src** folder of the new package, create a new file named **spin_server.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": [
    "roscd; cd ../src/spin_action_server/src"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "touch spin_server.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside this new file, copy the following content:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**spin_server.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "import actionlib\n",
    "from spin_action_server.msg import SpinAction, SpinGoal, SpinResult, SpinFeedback\n",
    "from nav_msgs.msg import Odometry\n",
    "from geometry_msgs.msg import Twist\n",
    "from tf import transformations\n",
    "import math\n",
    "\n",
    "class TurnRobotAction(object):\n",
    "    def __init__(self):\n",
    "        self._as = actionlib.SimpleActionServer(\"spin_server_X\", SpinAction, execute_cb=self.execute_cb, auto_start = False)\n",
    "        \n",
    "        self.odom_data = None\n",
    "        self.initial_orientation = None\n",
    "        self.cur_orientation = None\n",
    "        self._turn = False\n",
    "        self._turn_angle = None\n",
    "        self.cmd_pub = Twist()\n",
    "        self._feedback = SpinFeedback()\n",
    "        self._result = SpinResult()\n",
    "        \n",
    "        self.sub = rospy.Subscriber(\"/odom\", Odometry, self.odom_callback)\n",
    "        self.pub = rospy.Publisher(\"/cmd_vel\", Twist, queue_size=10 )\n",
    "        self._as.start()\n",
    "    \n",
    "    def execute_cb(self, goal):\n",
    "        r = rospy.Rate(30)\n",
    "        self._turn_angle = math.fabs((goal.angle * math.pi)/ 180)\n",
    "        rospy.loginfo(\"Target Robot Heading: %s\" % self._turn_angle)\n",
    "        \n",
    "        #set initial orientation\n",
    "        if not self.initial_orientation:\n",
    "            self.initial_orientation = self.cur_orientation\n",
    "            self._turn = True\n",
    "        \n",
    "        while (self.initial_orientation and self._turn):\n",
    "            #skip if initial and current orientation is same\n",
    "            if self.initial_orientation == self.cur_orientation:\n",
    "                continue\n",
    "            else:\n",
    "                #calculate the differences in orientation\n",
    "                cur_angle = transformations.euler_from_quaternion((self.cur_orientation.x, self.cur_orientation.y, \n",
    "                self.cur_orientation.z, self.cur_orientation.w))\n",
    "        \n",
    "                start_angle = transformations.euler_from_quaternion((self.initial_orientation.x, self.initial_orientation.y, \n",
    "                self.initial_orientation.z, self.initial_orientation.w))\n",
    "                \n",
    "                turn_so_far = math.fabs(cur_angle[2] - start_angle[2])\n",
    "                #rospy.loginfo(\"Current Robot turn at: %f\" % turn_so_far)\n",
    "                #send feedback (current Robot Heading)\n",
    "                self._feedback.heading = turn_so_far\n",
    "                self._as.publish_feedback(self._feedback)\n",
    "                rospy.loginfo(\"Current Robot Heading: %s\" % turn_so_far)\n",
    "                \n",
    "                if (turn_so_far >= self._turn_angle):\n",
    "                    #reset some variables if done turning\n",
    "                    self._turn = False\n",
    "                    self.initial_orientation = None\n",
    "                    self._result.done = \"done\"\n",
    "                    self._as.set_succeeded(self._result)\n",
    "                    rospy.loginfo(\"Turn successfully completed!\")\n",
    "        \n",
    "            #turn\n",
    "            if self._turn:\n",
    "                self.cmd_pub.linear.x = 0.0\n",
    "                if goal.angle > 0:\n",
    "                    self.cmd_pub.angular.z = goal.turn_speed\n",
    "                else:\n",
    "                    self.cmd_pub.angular.z = -goal.turn_speed\n",
    "                self.pub.publish(self.cmd_pub)\n",
    "            \n",
    "            else: #stop the robot before breaking out\n",
    "                self.cmd_pub.angular.z = 0.0\n",
    "                self.pub.publish(self.cmd_pub)\n",
    "                break\n",
    "            \n",
    "            r.sleep()\n",
    "    \n",
    "    def odom_callback(self, data): #get the odometry data\n",
    "        #get current orientation\n",
    "        self.cur_orientation = data.pose.pose.orientation\n",
    "        \n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    rospy.init_node('spin_node')\n",
    "    rospy.loginfo(\"Spin Server started ...\")\n",
    "    server = TurnRobotAction()\n",
    "    rospy.spin()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**spin_server.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can have a look at the code above and try to understand how it works. But basically, all you need to know is that it allows a robot to turn at a certain angle (specified by the user) at a certain speed (also specified by the user )on its z-axis. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) With the code for our Action Server created, let's now generate the messages it will use. First of all, let's create an **action** directory, where we will place the file that will define our message."
   ]
  },
  {
   "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 ../src/spin_action_server"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "mkdir action"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, create a file named **Spin.action** inside the **action** folder."
   ]
  },
  {
   "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 ../src/spin_action_server/action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "touch Spin.action"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Copy the below contents into it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Spin.action**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "float64 angle\n",
    "float64 turn_speed\n",
    "---\n",
    "string done\n",
    "---\n",
    "float64 heading"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Spin.action**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Now, before being able to compile the package, you'll need to do some modifications to the **CmakeLists.txt** and **package.xml** files."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### <span style=\"color: red;\">If you don't know exactly how to proceed here, you can have a look at Unit 4 (Part 2) of the ROS Basics in 5 Days Course. There you will find detailed instructions on how to modify these two files. </span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have minimum examples of how your files should look:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**CMakeLists.txt**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cmake_minimum_required(VERSION 2.8.3)\n",
    "project(spin_action_server)\n",
    "\n",
    "## Find catkin macros and libraries\n",
    "## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n",
    "## is used, also find other catkin packages\n",
    "find_package(catkin REQUIRED COMPONENTS\n",
    "  actionlib\n",
    "  actionlib_msgs\n",
    "  message_generation\n",
    "  message_runtime\n",
    "  rospy\n",
    ")\n",
    "\n",
    "## Generate actions in the 'action' folder\n",
    "add_action_files(\n",
    "   FILES\n",
    "   Spin.action\n",
    "#   Action2.action\n",
    ")\n",
    "\n",
    "## Generate added messages and services with any dependencies listed here\n",
    " generate_messages(\n",
    "   DEPENDENCIES\n",
    "   actionlib_msgs\n",
    " )\n",
    "\n",
    "catkin_package(\n",
    "#  INCLUDE_DIRS include\n",
    "  LIBRARIES spin_action_server\n",
    "  CATKIN_DEPENDS actionlib actionlib_msgs message_generation message_runtime rospy\n",
    "  DEPENDS system_lib\n",
    ")\n",
    "\n",
    "## Specify additional locations of header files\n",
    "## Your package locations should be listed before other locations\n",
    "# include_directories(include)\n",
    "include_directories(\n",
    "  ${catkin_INCLUDE_DIRS}\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**CMakeLists.txt**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**package.xml**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\"?>\n",
    "<package>\n",
    "  <name>spin_action_server</name>\n",
    "  <version>0.0.0</version>\n",
    "  <description>The spin_action_server package</description>\n",
    "  <maintainer email=\"user@todo.todo\">user</maintainer>\n",
    "  <license>TODO</license>\n",
    "\n",
    "  <buildtool_depend>catkin</buildtool_depend>\n",
    "  <build_depend>actionlib</build_depend>\n",
    "  <build_depend>actionlib_msgs</build_depend>\n",
    "  <build_depend>message_generation</build_depend>\n",
    "  <build_depend>message_runtime</build_depend>\n",
    "  <build_depend>rospy</build_depend>\n",
    "  <run_depend>actionlib</run_depend>\n",
    "  <run_depend>actionlib_msgs</run_depend>\n",
    "  <run_depend>message_runtime</run_depend>\n",
    "  <run_depend>message_generation</run_depend>\n",
    "  <run_depend>rospy</run_depend>\n",
    "\n",
    "  <export>\n",
    "  </export>\n",
    "\n",
    "</package>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**package.xml**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Finally, let's compile the new package so that all the messages are generated."
   ]
  },
  {
   "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 2.2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So... at this point, we have almost created the whole structure needed to execute our Actionlib state. This includes the **Action Server** that will provide the functionality to turn our robot and the **Actionlib State** that will connect to the Action Server in order to use that functionality. There's only one thing remainining...\n",
    "\n",
    "We need to add our new state to FlexBe! And that's exactly what we are going to do next."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Adding the Actionlib state to FlexBe"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's try to use our new state in FlexBe! For that, you'll need to launch the FlexBe application."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 2.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Launch the FlexBe application and add your new state, as shown in the previous chapter. You should have something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_turn1.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Now, reconnect the **failed** output to your new state."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_turn2.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, connect the **done** output of your new state back to the Drive State. You should end up with something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_turn3.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Finally, connect the **failed** output of your state to the **failed** end point."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<img src=\"img/flexbe_turn4.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Now, let's generate your parameters. Remember that for this new state, we had two parameters: the turning angle and the turning speed. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def __init__(self, turn_angle, t_speed):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Follow the same process as in the previous chapter. At the end, you should have something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_turn_params.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**IMPORTANT**: Remember to first click on the **Apply** button in order to apply your latest modifications to the behavior, and also click on the **Save Behavior** button to save your behavior."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "g) And finally, the last step will be to actually run our behavior. So, let's move to the **Runtime Control** window. You should see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_turn_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 as you like. So, let's set the turning angle to **90 degrees** and the turning speed to 0.4. Then, 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, once it's too close to the wall, stopping and doing a turn of 90 degrees. Then, it will continue moving forward the specified distance."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe3.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "**IMPORTANT**: Remember to first start the Action Server before executing your state. If you don't start the Action Server, your state will **NOT WORK**."
   ]
  },
  {
   "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 2.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Autonomy Levels"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "Just as the name implies, the Autonomy Level allows the operator to have complete control over the robot's behavior. Giving the robot some level of autonomy allows it to make its own decisions without operator involvement."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's now try to rerun the behavior from the previous exercise, but applying some Autonomy Levels to it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise 2.4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Launch the FlexBe application again and move to the **Statemachine Editor** panel. Once there, select your new Turning state and a panel will appear on the right side."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_autonomy1.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Now, modify the Autonomy Level of the **done** transition and set it to **High**. Remember to click the **Apply** button when finished."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_autonomy2.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Let's now go back to the **Runtime Control** panel. Execute the behavior again and see what happens."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe2.gif\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "Surprised? The robot has stopped right after doing the 90-degree turn. But why?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **Autonomy Level was the reason** why the behavior execution stopped during the first run. We just allowed transitions requiring less than low autonomy, which only applies to the most basic transitions. Each transition that required at least low autonomy was blocked and our second transition requires high autonomy as we configured in the last tutorial. This means that, as observed, the behavior will not execute the transition autonomously, but instead indicates that it is ready to be executed and waits for permission."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Now that we know a little bit more about what happened in the previous step, let's try to execute the behavior again. Now, when the robot stops after the turn, have a look at the **Runtime Control** window. You will see that there is a transition highlighted."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_autonomy3.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Allow the requested outcome by clicking on the highlighted arrow. You can click on outcomes at any time, even if they are not highlighted, in order to force them. The behavior will immediately return outcomes requested in this way, so it is your responsibility to decide whether an outcome should be forced. After permitting the outcome, the behavior will continue as usual. Interesting, right?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Let's have a look again at the **Runtime Control** panel. Here at the top-right corner of the screen, you will see a section like this one:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_autonomy4.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Within that section, you can read: **Block transitions which require at least X autonomy**. Set this value to **Full**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/flexbe_autonomy6.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "g) Execute your behavior again and see what happens now."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The behavior executed as usual, without requesting any permission for executing transitions, right? That's because we have increased the block level to **Full** so that it will only block transitions with a Full level of autonomy. Since our transition has an Autonomy Level of **High**, it won't be blocked now and everything will run as usual."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**End of Exercise 2.4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## Congratulations!! You are now capable of creating your own Actionlib state, and also managing the Autonomy Levels of your behaviors!"
   ]
  }
 ],
 "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
}