ROS_topics_and_more.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS workspaces, topics, servicies and actions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "## **Workspace**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before you start writing any ROS code, you need to set up a workspace for this code to\n",
    "live in. A workspace is simply a set of directories in which a related set of ROS code\n",
    "lives. You can have multiple ROS workspaces, but you can only work in one of them\n",
    "at any one time. The simple way to think about this is that you can only see code that\n",
    "lives in your current workspace.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Start by making sure that you’ve added the system-wide ROS setup script to\n",
    "your .bashrc file:\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ source /opt/ros/indigo/setup.bash\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we’re going to make a catkin workspace and initialize it:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ mkdir -p ~/catkin_ws/src\n",
    "user@hostname$ cd ~/catkin_ws/src\n",
    "user@hostname$ catkin_init_workspace\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This creates a workspace directory called catkin_ws (although you can call it anything\n",
    "you like), with a src directory inside it for your code. The catkin_init_workspace\n",
    "command creates a CMakeLists.txt file for you in the src directory, where you invoked\n",
    "it. 1 Next, we’re going to create some other workspace files:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ cd ~/catkin_ws\n",
    "user@hostname$ catkin_make\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Running catkin_make will generate a lot of output as it does its work. When it’s done,\n",
    "you’ll end up with two new directories: build and devel. build is where catkin is going\n",
    "to store the results of some of its work, like libraries and executable programs if you\n",
    "use C++. We’ll largely ignore build since we don’t need it much when using Python.\n",
    "devel contains a number of files and directories, the most interesting of which are the\n",
    "setup files. Running these configures your system to use this workspace, and the code\n",
    "that’s (going to be) contained inside it. Assuming you’re using the default command-\n",
    "line shell ( bash ) and are still in the top-level directory of your workspace, you can do\n",
    "this with:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ source devel/setup.bash\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Congratulations! You’ve just created your first ROS workspace. You should put all the\n",
    "code for this book, and any additional code you write that’s based on it, into this\n",
    "workspace, in the src directory, organized as ROS packages."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## ROS Packages"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "ROS software is organized into packages, each of which contains some combination\n",
    "of code, data, and documentation.\n",
    "Packages sit inside workspaces, in the src directory. Each package directory must\n",
    "include a CMakeLists.txt file and a package.xml file that describes the contents of the\n",
    "package and how catkin should interact with it. Creating a new package is easy:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ cd ~/catkin_ws/src\n",
    "user@hostname$ catkin_create_pkg my_awesome_code rospy\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This changes the directory to src (where packages live) and invokes catkin_cre\n",
    "ate_pkg to make the new package called my_awesome_code , which depends on the\n",
    "(already existing) rospy package. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## roslaunch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "oslaunch is a command-line tool designed to automate the launching of collections\n",
    "of ROS nodes. On the surface, it looks a lot like rosrun , needing a package name and\n",
    "a filename:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ roslaunch PACKAGE LAUNCH_FILE\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "However, roslaunch operates on launch files, rather than nodes. Launch files are\n",
    "XML files that describe a collection of nodes along with their topic remappings and parameters. By convention, these files have a suffix of `.launch`. For example, here is\n",
    "`talker_listener.launch` in the rospy_tutorials package:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```xml\n",
    "<launch>\n",
    "    <node name=\"talker\" pkg=\"rospy_tutorials\"\n",
    "        type=\"talker.py\" output=\"screen\" args = \"1\"/>\n",
    "    <node name=\"listener\" pkg=\"rospy_tutorials\"\n",
    "        type=\"listener.py\" output=\"screen\" args = \"2\"/>\n",
    "</launch>\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Each <node> tag includes attributes declaring the ROS graph name of the node, the\n",
    "package in which it can be found, and the type of node, which is simply the filename\n",
    "of the executable program. In this example, the output=\"screen\" attributes indicate\n",
    "that the talker and listener nodes should dump their console outputs to the cur‐\n",
    "rent console, instead of only to log files. This is a commonly used setting for debug‐\n",
    "ging; once things start working, it is often convenient to remove this attribute so that\n",
    "the console has less noise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Topics**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As we saw in the previous chapter, ROS systems consist of a number of independent\n",
    "nodes that comprise a graph. These nodes by themselves are typically not very useful.\n",
    "Things only get interesting when nodes communicate with each other, exchanging\n",
    "information and data. The most common way to do that is through topics. A topic is a\n",
    "name for a stream of messages with a defined type. For example, the data from a laser\n",
    "range-finder might be sent on a topic called scan , with a message type of LaserScan ,\n",
    "while the data from a camera might be sent over a topic called image , with a message\n",
    "type of Image .\n",
    "Topics implement a publish/subscribe communication mechanism, one of the more\n",
    "common ways to exchange data in a distributed system. Before nodes start to trans‐\n",
    "mit data over topics, they must first announce, or advertise, both the topic name and\n",
    "the types of messages that are going to be sent. Then they can start to send, or publish,\n",
    "the actual data on the topic. Nodes that want to receive messages on a topic can sub‐\n",
    "scribe to that topic by making a request to roscore . After subscribing, all messages on\n",
    "the topic are delivered to the node that made the request. One of the main advantages\n",
    "to using ROS is that all the messy details of setting up the necessary connections\n",
    "when nodes advertise or subscribe to topics is handled for you by the underlying\n",
    "communication mechanism so that you don’t have to worry about it yourself.\n",
    "In ROS, all messages on the same topic must be of the same data type. Although ROS\n",
    "does not enforce it, topic names often describe the messages that are sent over them.\n",
    "For example, on the PR2 robot, the topic /wide_stereo/right/image_color is used\n",
    "for color images from the rightmost camera of the wide-angle stereo pair."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```python\n",
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "from std_msgs.msg import Int32\n",
    "rospy.init_node('topic_publisher')\n",
    "pub = rospy.Publisher('counter', Int32)\n",
    "rate = rospy.Rate(2)\n",
    "count = 0\n",
    "while not rospy.is_shutdown():\n",
    "    pub.publish(count)\n",
    "    count += 1\n",
    "    rate.sleep()\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first line:\n",
    "\n",
    "`#!/usr/bin/env python`\n",
    "\n",
    "is known as the shebang. It lets the operating system know that this is a Python file,\n",
    "and that it should be passed to the Python interpreter. Since we’re going to be running\n",
    "the nodes we write as programs, we also have to set execute permissions on them\n",
    "using the Linux chmod command:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ chmod u+x topic_publisher.py\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Help commands to understand what is going on in the topic:\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ rostopic echo TOPIC_NAME -n 5\n",
    "user@hostname$ rostopic hz TOPIC_NAME\n",
    "user@hostname$ rostopic info TOPIC_NAME\n",
    "user@hostname$ rostopic find std_msgs/Int32\n",
    "user@hostname$ rostopic pub TOPIC_NAME std_msgs/Int32 1000000\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If a topic is marked as **latched**\n",
    "when it is advertised, subscribers automatically get the last message sent when they\n",
    "subscribe to the topic. In our map_server example, this means that we only need to\n",
    "mark it as latched and publish it once. Topics can be marked as latched with the\n",
    "optional latched argument:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```python\n",
    "pub = rospy.Publisher('map', nav_msgs/OccupancyGrid, latch=True)\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Services**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Services are another way to pass data between nodes in ROS. Services are just syn‐\n",
    "chronous remote procedure calls; they allow one node to call a function that executes\n",
    "in another node. We define the inputs and outputs of this function similarly to the\n",
    "way we define new message types. The server (which provides the service) specifies a\n",
    "callback to deal with the service request, and advertises the service. The client (which\n",
    "calls the service) then accesses this service through a local proxy.\n",
    "Service calls are well suited to things that you only need to do occasionally and that\n",
    "take a bounded amount of time to complete. Common computations, which you\n",
    "might want to distribute to other computers, are a good example. Discrete actions\n",
    "that the robot might do, such as turning on a sensor or taking a high-resolution pic‐\n",
    "ture with a camera, are also good candidates for a service-call implementation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step in creating a new service is to define the service call inputs and outputs.\n",
    "This is done in a service-definition file, which has a similar structure to the message-\n",
    "definition files we’ve already seen.\n",
    "The inputs to the service call come first. In this case, we’re just going to use the ROS\n",
    "built-in string type. Three dashes ( --- ) mark the end of the inputs and the start of\n",
    "the output definition."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For example:\n",
    "```\n",
    "string words\n",
    "---\n",
    "uint32 count\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we need to make some changes in `CMakeLists.txt` and `package.xml` files **inside package directory**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "Inside `CMakeLists.txt`:\n",
    "```python\n",
    "...\n",
    "...\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",
    "  rospy\n",
    "  std_msgs\n",
    "  message_generation # Add message_generation here, after the other packages\n",
    ")\n",
    "...\n",
    "...\n",
    "...\n",
    "## Generate services in the 'srv' folder\n",
    "add_service_files(\n",
    "  FILES # put it as it is\n",
    "  YOUR_SERVICE_NAME.srv\n",
    "  #Service2.srv\n",
    ")\n",
    "...\n",
    "...\n",
    "...\n",
    "generate_messages(\n",
    "  DEPENDENCIES\n",
    "  std_msgs  # Or other packages containing msgs\n",
    ")\n",
    "...\n",
    "...\n",
    "...\n",
    "\n",
    "```\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "Inside `package.xml`:\n",
    "```xml\n",
    "...\n",
    "...\n",
    "...\n",
    "  <build_depend>message_generation</build_depend>\n",
    "  <exec_depend>message_runtime</exec_depend>\n",
    "...\n",
    "...\n",
    "...\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "And after all that that run \n",
    "```\n",
    "$ catkin_make install\n",
    "```\n",
    "command in WS directory.\n",
    "With all of this in place, running `catkin_make` will generate three classes: `WordCount` ,\n",
    "`WordCountRequest` , and `WordCountResponse` . These classes will be used to interact\n",
    "with the service, as we will see."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can verify that the service call definition is what we expect by using the rossrv\n",
    "command:\n",
    "```\n",
    "user@hostname$ rossrv show YOUR_SERVICE_NAME\n",
    "[basics/YOUR_SERVICE_NAME]:\n",
    "string words\n",
    "---\n",
    "uint32 count\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can see all available services using rossrv list , all packages offering services\n",
    "with rossrv packages , and all the services offered by a particular package with\n",
    "rossrv package ."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Code of ```service_server.py```:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```python\n",
    "#!/usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "\n",
    "from sample_code.srv import WordCount, WordCountResponse\n",
    "\n",
    "\n",
    "def count_words(request):\n",
    "    return len(request.words.split())\n",
    "\n",
    "rospy.init_node('service_server')\n",
    "\n",
    "servise = rospy.Service('word_count', WordCount, count_words)\n",
    "\n",
    "rospy.spin()\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we make a call to rospy.spin() , which gives control of the node over to ROS\n",
    "and exits when the node is ready to shut down. You don’t actually have to hand con‐\n",
    "trol over by calling rospy.spin() (unlike in the C++ API), since callbacks run in\n",
    "their own threads. You could set up your own loop, remembering to check for node\n",
    "termination, if you have something else you need to do. However, using\n",
    "rospy.spin() is a convenient way to keep the node alive until it’s ready to shut down."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that we have the service defined and implemented, we can verify that everything\n",
    "is working as expected with the rosservice command. Start up a roscore and run\n",
    "the service node:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ rosrun PACKAGE_NAME service_server.py\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, let’s check that the service is there:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```\n",
    "user@hostname$ rosservice list\n",
    "/rosout/get_loggers\n",
    "/rosout/set_logger_level\n",
    "/service_server/get_loggers\n",
    "/service_server/set_logger_level\n",
    "/word_count\n",
    "\n",
    "user@hostname$ rosservice info word_count\n",
    "Node: /service_server\n",
    "URI: rosrpc://hostname:60085\n",
    "Type: basics/WordCount\n",
    "Args: words\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The simplest way to use a service is to call it using the rosservice command. For our\n",
    "word-counting service, the call looks like this:\n",
    "```\n",
    "user@hostname$ rosservice call word_count 'one two three'\n",
    "count: 3\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Code of `service_client.py`:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```python\n",
    "#!/usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from sample_code.srv import WordCount\n",
    "import sys\n",
    "\n",
    "rospy.init_node('service_client')\n",
    "rospy.wait_for_service('word_count')\n",
    "word_counter = rospy.ServiceProxy('word_count', WordCount)\n",
    "words = ''.join(sys.argv[1:])\n",
    "word_count = word_counter(words)\n",
    "print(words, '->', word_count.count)\n",
    "\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Checking the results:\n",
    "```\n",
    "user@hostname$ rosrun PACHAGE_NAME service_client.py these are some words\n",
    "these are some words -> 4\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## **Actions**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Imagine what a service interface to goto_position might look like to the caller: you\n",
    "send a request containing the goal location, then you wait for an indeterminate\n",
    "amount of time to receive the response that tells you what happened. While waiting,\n",
    "your calling program is forced to block, you have no information about the robot’s\n",
    "progress toward the goal, and you can’t cancel or change the goal. To address these\n",
    "shortcomings, ROS provides actions."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "ROS actions are the best way to implement interfaces to time-extended, goal-oriented\n",
    "behaviors like goto_position . While services are synchronous, actions are asynchro‐\n",
    "nous. Similar to the request and response of a service, an action uses a goal to initiate\n",
    "a behavior and sends a result when the behavior is complete. But the action further\n",
    "uses feedback to provide updates on the behavior’s progress toward the goal and also\n",
    "allows for goals to be canceled. Actions are themselves implemented using topics. An\n",
    "action is essentially a higher-level protocol that specifies how a set of topics (goal,\n",
    "result, feedback, etc.) should be used in combination."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We create an `action` folder in package directory and then create there, for example, `Timer.action` file:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```xml\n",
    "# goal\n",
    "duration time_to_wait\n",
    "---\n",
    "# result\n",
    "duration time_elaspsed\n",
    "uint32 updates_sent\n",
    "---\n",
    "# feedback\n",
    "duration time_elapsed\n",
    "duration time_remaining\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "Changes in `CMakeLists.txt`:\n",
    "```python\n",
    "...\n",
    "...\n",
    "...\n",
    "find_package(catkin REQUIRED COMPONENTS\n",
    "  rospy\n",
    "  std_msgs\n",
    "  actionlib_msgs\n",
    "  message_generation # add this at the end\n",
    ")\n",
    "...\n",
    "...\n",
    "...\n",
    "## Generate actions in the 'action' folder\n",
    "add_action_files(\n",
    "  DIRECTORY action\n",
    "  FILES Timer.action\n",
    "#  Action1.action\n",
    "#  Action2.action\n",
    ")\n",
    "...\n",
    "...\n",
    "...\n",
    "## Generate added messages and services with any dependencies listed here\n",
    "generate_messages(\n",
    "  DEPENDENCIES\n",
    "  actionlib_msgs\n",
    "  std_msgs  # Or other packages containing msgs\n",
    ")\n",
    "...\n",
    "...\n",
    "...\n",
    "catkin_package(\n",
    "  CATKIN_DEPENDS actionlib_msgs\n",
    "#  INCLUDE_DIRS include\n",
    "#  LIBRARIES sample_code\n",
    "#  CATKIN_DEPENDS rospy\n",
    "#  DEPENDS system_lib\n",
    ")\n",
    "...\n",
    "...\n",
    "...\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "Changes in `package.xml` file:\n",
    "```xml\n",
    "...\n",
    "...\n",
    "...  \n",
    "  <build_depend>actionlib_msgs</build_depend>\n",
    "  <exec_depend>actionlib_msgs</exec_depend>\n",
    "  <build_depend>message_generation</build_depend>\n",
    "  <exec_depend>message_runtime</exec_depend>\n",
    "  <exec_depend>message_generation</exec_depend>\n",
    "...\n",
    "...\n",
    "...\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The code of `fancy_action_server.py`: "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```python\n",
    "#! /usr/bin/env python\n",
    "import rospy\n",
    "import time\n",
    "import actionlib\n",
    "from sample_code.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback\n",
    "\n",
    "def do_timer(goal):\n",
    "    start_time = time.time()\n",
    "    update_count = 0\n",
    "    if goal.time_to_wait.to_sec() > 60.0:\n",
    "        result = TimerResult()\n",
    "        result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)\n",
    "        result.updates_sent = update_count\n",
    "        server.set_aborted(result, \"Timer aborted due to too-long wait\")\n",
    "        return\n",
    "    while (time.time() - start_time) < goal.time_to_wait.to_sec():\n",
    "        if server.is_preempt_requested():\n",
    "            result = TimerResult()\n",
    "            result.time_elapsed = \\\n",
    "            rospy.Duration.from_sec(time.time() - start_time)\n",
    "            result.updates_sent = update_count\n",
    "            server.set_preempted(result, \"Timer preempted\")\n",
    "            return\n",
    "        feedback = TimerFeedback()\n",
    "        feedback.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)\n",
    "        feedback.time_remaining = goal.time_to_wait - feedback.time_elapsed\n",
    "        server.publish_feedback(feedback)\n",
    "        update_count += 1\n",
    "        time.sleep(1.0)\n",
    "\n",
    "    result = TimerResult()\n",
    "    result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)\n",
    "    result.updates_sent = update_count\n",
    "    server.set_succeeded(result, \"Timer completed successfully\")\n",
    "\n",
    "\n",
    "rospy.init_node('timer_action_server')\n",
    "server = actionlib.SimpleActionServer('timer', TimerAction, do_timer, False)\n",
    "server.start()\n",
    "rospy.spin()\n",
    "\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that we have implemented the action server, we can do a couple of checks to\n",
    "ensure that it’s working as expected. Start up a roscore and then run the action\n",
    "server:\n",
    "```\n",
    "user@hostname$ rosrun basics fancy_action_server.py\n",
    "user@hostname$ rostopic list\n",
    "/rosout\n",
    "/rosout_agg\n",
    "/timer/cancel\n",
    "/timer/feedback\n",
    "/timer/goal\n",
    "/timer/result\n",
    "/timer/status\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What’s a TimerActionGoal ? Let’s dig in further, now with rosmsg :\n",
    "```\n",
    "user@hostname$ rosmsg show TimerActionGoal\n",
    "[basics/TimerActionGoal]:\n",
    "std_msgs/Header header\n",
    "uint32 seq\n",
    "time stamp\n",
    "string frame_id\n",
    "actionlib_msgs/GoalID goal_id\n",
    "time stamp\n",
    "string id\n",
    "basics/TimerGoal goal\n",
    "duration time_to_wait\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Interesting; we can see our goal definition in there, as the goal.time_to_wait field,\n",
    "but there are also some extra fields that we didn’t specify. Those extra fields are used\n",
    "by the action server and client code to keep track of what’s happening. Fortunately,\n",
    "that bookkeeping information is automatically stripped away before our server code\n",
    "sees a goal message. While a TimerActionGoal message is sent over the wire, what we\n",
    "see in our goal execution is a bare TimerGoal message, which is just what we defined\n",
    "in our .action file:\n",
    "```\n",
    "user@hostname$ rosmsg show TimerGoal\n",
    "[basics/TimerGoal]:\n",
    "duration time_to_wait\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The code of `fancy_action_client.py`:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "```python\n",
    "#! /usr/bin/env python\n",
    "import rospy\n",
    "import time\n",
    "import actionlib\n",
    "from sample_code.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback\n",
    "\n",
    "\n",
    "def feedback_cb(feedback):\n",
    "    print('[Feedback] Time elapsed: %f'%(feedback.time_elapsed.to_sec()))\n",
    "    print('[Feedback] Time remaining: %f'%(feedback.time_remaining.to_sec()))\n",
    "\n",
    "\n",
    "rospy.init_node('timer_action_client')\n",
    "client = actionlib.SimpleActionClient('timer', TimerAction)\n",
    "client.wait_for_server()\n",
    "goal = TimerGoal()\n",
    "goal.time_to_wait = rospy.Duration.from_sec(5.0)\n",
    "# Uncomment this line to test server-side abort:\n",
    "#goal.time_to_wait = rospy.Duration.from_sec(500.0)\n",
    "client.send_goal(goal, feedback_cb=feedback_cb)\n",
    "# Uncomment these lines to test goal preemption:\n",
    "#time.sleep(3.0)\n",
    "#client.cancel_goal()\n",
    "client.wait_for_result()\n",
    "print('[Result] State: %d'%(client.get_state()))\n",
    "print('[Result] Status: %s'%(client.get_goal_status_text()))\n",
    "print('[Result] Time elapsed: %f'%(client.get_result().time_elapsed.to_sec()))\n",
    "print('[Result] Updates sent: %d'%(client.get_result().updates_sent))\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "## **Summary**\n",
    "Comparison of topics, services, and actions:\n",
    "\n",
    "**Topic** \n",
    "\n",
    "One-way communication, especially if there might be multiple nodes listening (e.g., streams of sensor data)\n",
    "\n",
    "**Service**\n",
    "\n",
    "Simple request/response interactions, such as asking a question about a node’s current state\n",
    "\n",
    "**Action**\n",
    "\n",
    "Most request/response interactions, especially when servicing the request is not instantaneous (e.g., navigating to a\n",
    "goal location)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.8.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}