State Machines to Complete Missions

Breaking a complex problem up into smaller, more manageable subproblems is a core technique used in solving today's challenges. One way to represent and think about each subproblem is to think of states. Solving the problem is then simply transitioning between each of the subproblems (states) in the right order. A state is a description of the given subproblem/process at that point in time. For instance, a human might be in a hungry state. A transition is a link that allows changes between states. For instance, a transition might be getting up and going to the kitchen so that a human can transition from a hungry state to an eating state. Modeling problems as a set of states and transitions have been used to model problems in many fields, including mathematics, engineering, and artificial intelligence. Think, for instance, some of the hardest engineering problems known to man. Consider flying into space. A rocket generally consists of multiple stages (states). At each of these states, the rocket is performing a set action when the rocket is given an input, the rocket transitions from one state to the next. Below is an example of the 2001 Mars Odyssey spacecraft, NASA's longest-lasting spacecraft at Mars. The diagram below is an example from the launch sequence of each state and state transition the rocket performed.

Learning Objectives

At the end of this lab, you should understand:

• How to add nodes to an existing system
• How to keep rich logs
• How to track states with a finite-state automaton
• How to use ROS messages

Updating to FastSim

You can find the details on how to install and setup our lightweight fastsim simulator on this Github page. Once you have installed the simulator, make sure you test it as shown in the installation instructions! Once you are sure it works, you need to do the following to get ready to do Lab 3.

Clone the keyboard package:

# Go into the source folder of our workspace

$ cd ~/fastsim/src

# Clone the keyboard package

$ git clone https://github.com/lrse/ros-keyboard.git

Create a package called simple_control in the fastsim workspace:

# Go into the source folder of our workspace

$ cd ~/fastsim/src

# Create the simple_control package

$ catkin_create_pkg simple_control std_msgs rospy

Create a keyboard_manager node. (Note: the code you need to put in keyboard manager is given later in this lab)

# Create the keyboard-to-abort manager node

$ touch ~/fastsim/src/simple_control/src/keyboard_manager.py

# Give the keyboard-to-abort manager node execution privileges

$ chmod u+x ~/fastsim/src/simple_control/src/keyboard_manager.py

Finally, we can build the workspace using:

# Go into workspace

$ cd ~/fastsim

# Build the workspace

$ catkin build

Important Note: If you are using fastsim, whenever you read crazysim_ws, replace that with fastsim. When the code necessary for crazysim_ws and fastsim differs, we will make both sets of code available.

Improving the Drone Control Software

In Lab 2, we implemented a keyboard manager that forwarded the commands onto the drone without considering any form of state or safety precautions. Setting up a keyboard manager like this is good because it keeps the code simple and efficient. However, there are downsides:

1) The robot has no concept of a state. Imagine the quadrotor was trying to take a picture, and during that time, it received a move command. It would start moving even though it's busy taking a picture, causing the final image to be ruined.

2) There are no safety checks. We might know that an obstacle or boundary exists. However, without safety checks, regardless of these facts, the keyboard commands would be forwarded onto the drone.

In this lab, we start by addressing these two issues. To address these issues, we will explicitly define part of a drone state and a safety node as seen below:

Create the State and Safety Node

The first step in improving the drone's control software will be to create a state and safety node. We will start the implementation of this node by only considering the first objective:

• Track the state of the drone

If you are using crazysim_ws , we will build on what you did in Lab 2. If you are using the lighter simulator fastsim , we will be working in the fastsim workspace.

To track the drone's state, we are going to need to create a new node in the simple_control package. Create a new node in simple_control package in the crazysim_ws workspace called state_and_safety.py using what you have learned from Lab 1 and Lab 2.

Note: remember to give the node execution permissions using the chmod command.

The control software of a robot operation can be complex, and it can quickly become unmanageable. To manage this complexity, a Finite State Automaton (FSA) is a natural way to partition the problem into states and state transitions. An FSA is a mathematical computation model that can be in exactly one of a finite number of states at any given time. This is a conceptualization of the system as always being in one of several states. The system makes well-defined moves from one state to another in response to inputs or events.

Using an FSA we will design the state_and_safety.py node as follows (Note: If you are using fastsim, the output topic will be /uav/input/position instead of /cf1/cmd_position):

The state_and_safety.py will accept a position from the keyboard manager. If we are in state 1, the hovering state, we will transition into a moving state. The moving state will move the quadrotor to the requested position. When the drone reaches the required position, the FSA will transition back to state 1, the hovering state, and wait for the next input. There are 4 main design considerations to take into account:

1. Keep track of the drone's state.
2. Encode state transitions.
3. In lab 2, our keyboard manager integrated directly with the drone on the topic /cf1/cmd_position. We will now need for the keyboard manager to publish positions instead (the desired input to the FSA).
4. Tell when quadrotor has reached the desired position so that it can transition from back to state 1.

To implement each of these points, we will do the following:

Fulfilling 1) We will implement a state machine. We can do that in many ways (e.g., nested switches, table mapping, state pattern), but for simplicity and given the number of states, we will use a sequence of nested predicates and start by declaring an enumeration that represents the possible states. An enumeration is a set of symbolic names (members) bound to unique, constant values.

# A class to keep track of the quadrotors state

class DroneState(Enum):

  HOVERING = 1

  VERIFYING = 2

  MOVING = 3

Fulfilling 2) We will break the code up into a set of discrete functions. Each of the functions will represent the drone behavior within a state. We can then track the states using a class variable and call the correct function based on the state. For example, if the quadrotor is in the MOVING state, then the processMoving() function will be called.

# Check if the drone is in a moving state

if self.state == DroneState.MOVING:

  self.processMoving()

# If we are hovering then accept keyboard commands

elif self.state == DroneState.HOVERING:

  self.processHovering()

Fulfilling 3) We will subscribe to the /keyboardmanager/position topic using a subscriber. (Later in the lab we will adjust the keyboard manager so that it publishes the correct messages)

rospy.Subscriber('/keyboardmanager/position', Position, self.getKeyboardCommand, queue_size = 1)

If you are using fastsim the subscriber will look as follows:

rospy.Subscriber('/keyboardmanager/position', Vector3, self.getKeyboardCommand, queue_size = 1)

Fulfilling 4) We will need a way to get the drone's current position (we will do this after Checkpoint 1) to transition back from MOVING to HOVERING. Assuming we will have that information, we can implement a computation and a check to see how far the drone is away from the desired goal. If the quadrotor is inside an acceptance-range, then the machine can transition back into a hovering state:

dx = self.goal_cmd.x - self.drone_position.x

dy = self.goal_cmd.y - self.drone_position.y

dz = self.goal_cmd.z - self.drone_position.z

# Euclidean distance

distance_to_goal = sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))

# If goal is reached transition to hovering

if distance_to_goal < self.acceptance_range:

  self.state = DroneState.HOVERING

  ...

The Code for the State and Safety node

Putting it all together: copy and paste the following code into the state_and_safety.py node. You will notice that we are using the previous points in the code and a few others that we discuss next.

A useful resource to notice: ROS Logging

You will notice that this code no longer uses a standard print command. Each of the print commands has been replaced with a rospy.loginfo(str(rospy.get_name()) + "...") command. Using a log command in ROS is a good practice. A print command will print a message to the terminal, with no extra logging. A rospy.loginfo() command will print a message to the terminal, as well as keep that message inside the ROS logs. That way, you can go back and review your robot's behavior at a later stage. We also added the following string to each log: str(rospy.get_name()). This is beneficial when there are more than two nodes printing messages to the terminal, as we will be able to differentiate messages from separate nodes more easily. ROS logging allows you to create levels of messages so that important messages and less important messages can be distinguished. The most important messages are called fatal messages. To publish a fatal message, you use the command rospy.logfatal(). The lowest level of a message is a debug message which can be logged using rospy.logdebug(). More on ROS logging can be found on the ROS Wiki.

Updating the Keyboard Manager

Now let's make the changes to the keyboard_manager node to fulfill requirement #2. The keyboard_manager node needs to change in a few ways:

1. It now needs to publish messages of type position on the topic /keyboardmanager/position.
2. It should only publish a message once a position is set instead of continuously publishing any keyboard input. To do this, we will change the keyboard_manager only to publish messages when the user hits enter on their keyboard.
3. The keyboard_manager will display the position to be sent onto the terminal using logging.

To address each of these changes, we will make the following updates to the keyboard_manager.

Fulfilling 1) Change what the node publisher by updating the publishing statement:

self.position_pub = rospy.Publisher('/keyboardmanager/position', Position, queue_size=1)

If you are using fastsim the code will look as follows (note the different message type):

self.position_pub = rospy.Publisher('/keyboardmanager/position', Vector3, queue_size=1)

Fulfilling 2) Change the main loop to only publish messages when the user hits enter:

# If the user presses the enter key

if self.key_code == 13:

  # Publish the position

  self.position_pub.publish(self.pos)

  rospy.loginfo(str(rospy.get_name()) + ": Sending Position")

Fulfilling 3) Log positions typed when they change. If the prev_pos and pos do not match, the new pos has changed:

check_x = self.prev_pos.x != self.pos.x

check_y = self.prev_pos.y != self.pos.y

check_z = self.prev_pos.z != self.pos.z

if check_x or check_y or check_z:

  self.prev_pos = copy.deepcopy(self.pos)

  rospy.loginfo(str(rospy.get_name()) + ": Keyboard: " + self.goalToString(self.pos))

The Keyboard Manager Code

When you implement these changes your new keyboard_manager should look like this (note you will need the code completed in Lab 2 checkpoint 4):

Putting it All Together

The last thing we need to do is to change our launch file so that we can run each of the nodes. To do that, add each node to the roslaunch file.

If you are using crazysim, the simulator roslaunch file is: ~/crazysim_ws/src/course_packages/dd2419_simulation/launch/simulator.launch

If you are using fastsim, the launch file is located in: ~/fastsim/src/flightcontroller/launch/fly.launch

...

  <node name="state_safety_node" pkg="simple_control" type="state_and_safety.py" output="screen"/>

  <node name="keyboard_manager_node" pkg="simple_control" type="keyboard_manager.py" output="screen"/>

  <node name="keyboard" pkg="keyboard" type="keyboard"/>

...

Message Types

Let's implement the drone's position tracking. Start by figuring out what topic the drone's position is being published on. To do this, start the simulation and list the available topics:

In the case of the fastsim:

# Fastsim

$ rostopic list

...

/uav/sensors/filtered_imu

/uav/sensors/gps

/uav/sensors/imu

...

In the case of the crazysim:

# Crazysim

$ rostopic list

...

/cf1/external_position

/cf1/pose

...

The list of available topics should look as shown above. As you become more familiar with robotics in ROS, you will start to work with a set of common message types. This was purposefully done by the ROS developers to try and keep a set of standard messages and thus increase compatibility between different nodes, and allow for faster development.

One of the standard ROS message types is Pose. A pose represents a robot's position and orientation. In crazysim you will notice that one of the available topics is called /cf1/pose. If you are looking at fastsim, you will notice a topic called /uav/sensors/gps. This gives us a hint that this might contain the drone's position and orientation. Let's identify what this topic is publishing and what message type it is. First, let's see if this is the topic we want. Echo the topic:

In the case of fastsim:

# Fastsim

$ rostopic echo /uav/sensors/gps

...

pose: 

  position: 

    x: -0.01205089835

    y: -0.014817305419

    z: 0.548691896515

  orientation: 

    x: 0.000119791513234

    y: -8.21647300556e-05

    z: -0.000329261478202

    w: 0.999999935243

---

...

#----------------------------

In the case of crazysim:

# Crazysim

$ rostopic echo /cf1/pose

...

pose: 

  position: 

    x: -0.01205089835

    y: -0.014817305419

    z: 0.548691896515

  orientation: 

    x: 0.000119791513234

    y: -8.21647300556e-05

    z: -0.000329261478202

    w: 0.999999935243

---

...

Nice! Just as we expected, there is a position and an orientation. Lets now try and figure out what type this message is so that we can read the documentation and subscribe to it. To find more information about a topic, you can use another useful command: rostopic info. This command prints all the information for a given topic. Run the command:

In the case of fastsim:

# Fastsim

$ rostopic info /uav/sensors/gps 

Type: geometry_msgs/PoseStamped

Publishers: 

 * /uav/flightgoggles_uav_dynamics (http://robotclass:46391/)

Subscribers: 

 * /position_controller_node (http://robotclass:42737/)

 * /view_node (http://robotclass:46667/)

In the case of crazysim:

# Crazysim

$ rostopic info /cf1/pose

Type: geometry_msgs/PoseStamped

Publishers: 

 * /gazebo_pose_publisher (http://robotclass:44867/)

Subscribers: None

We can see that this message is of type geometry_msgs/PoseStamped.

Now that we know the message type, let's look at how it works on the ROS wiki. Close the simulator for now. Do a quick search: "ROS message geometry_msgs PoseStamped" depending on your search engine, you should find this webpage in the top few search results.

Using the documentation for reference, we can see that it is a standard ROS message that consists of a header as well as a pose. Clicking on the links will allow you to see how this message is composed. Below is a diagram of the message constructed by visiting each of the links:

You will see that a pose consists of a position and an orientation. Interestingly the position is of type Pose, which is also declared in geometry_msgs. Opening the message, Pose reveals that it consists of a position and orientation. The position is a geometry_msg, Point. Following this logic, we find that the message type Point consists of three floats x, y, and, z. This method of identifying common messages can be repeated. In general, it is better to use standard ROS messages, as it allows faster development times and standardizes the messages sent on topics for easier integration between different projects. It is analogous to cellphones, all using standard communication standards (within a country at least). If every company would design their own, keeping track of which phone comm to use would be a nightmare.

Note: There are ways to create custom message types, some of which crazysim has used. You can find more information on this in the extra section. If possible, standard message usage is recommended.

Adding Position Tracking to enable the Transition from Moving to Hovering

Using what we learned with message types, let's adapt our state_and_safety.py node to subscribe to the Pose topic published by the simulator. Doing this will allow us to monitor when the drone reaches a goal and transitions from state MOVING back to state HOVERING.

In the state_and_safety.py node, write code to fill in the two #TODO sections.

First, in the node initialization, subscribe to the drone's pose using the correct topic name, and message type. Direct each of the messages to a callback.

Second, implement the callback for this message. The callback should store the drone's position to the class variable self.drone_position. You can access each component of the message using standard dot notation. For instance, if I wanted to get the orientation of the quadrotor, I could use:

msg.pose.orientation

Where, msg is passed into the callback as a function parameter.

Congratulations you are done with Lab 3.

To Check

1. Show the computation graph for checkpoint 1
   1. Try and fly the drone using the keyboard
      1. What happens when you hit the enter key? Answer this in terms of the FSA states we implemented.
      2. What happens when you request the drone to fly to a second position? Answer this in terms of the actual code used in state_and_safety.py.
2. Show the computation graph for checkpoint 2
   1. Try and fly the drone using the keyboard
      1. What happens when you request the drone to fly to a second position?
      2. Send a message to the drone while it is moving to a target position. What happens? Why?

Extra:

More information on custom message types.

More information on adding command-line arguments to a launch file.