RSECS4501-Spring2020

Lab 6 - PID Controllers and ROS Testing

Introduction

For a brief introduction to the Proportional-Integral-Derivative (PID) Controller watch the video below.

The PID Controller is a control algorithm that uses an error measurement to adjust the system's output with the goal of maintaining a set point.

The error is the difference between the setpoint, r(t), and the output, y(t).

The proportional term is simply a constant multiplied by the error. Thus, the proportional term applies a correction that is proportional to the error. It works because as the error gets smaller, the correction also gets smaller. However, if an external disturbance is applied to the system, for example an wind steadily pushing the drone in one direction, then the error term does not approach zero, but stays above a certain threshold. The P controller is not able to compensate for this type of error and it will overshoot, undershoot or oscillate around the set point.

The integral term is a constant multiplied by the integral over the interval [0,t] of the error. This term accumulates the error over time, and compensates for the offset that might be caused by an external disturbance. The image below shows the effect of a PD controller on the signal.

The integral controller essentially brings the area between the set point and the signal to zero. The effect is that any offset is eliminated. However, if the signal oscillates like in the picture below, then the area is zero, but the output signal is not stable.

The derivative term is equal to a constant multiplied with the derivative of the error at time t. The derivative term corrects the errors caused by rapid variations of the output signal.

The sum of the three terms, proportional, integral and derivative, is the control signal, u(t). The control signal is an input of the process and the output is y(t), which is used to calculate the error, thus closing the control loop.

In practice, the signal is measured at discrete time intervals, and the derivative and integral are approximated. To program the PID controller we use the following equations:

Goal

In this lab, we implement a PID controller that aims at bringing the yaw angle to a set point value and maintain it. The yaw angle is the rotation around the z-axis as shown in the image below.

Learning Objectives

At the end of this lab, you should:

Implement a PID controller
Learn the basics of the ROS testing framework rostest

Update Fastsim

Note: if you are using the lightweight version of the simulator (fastersim) replace fastsim with fastersim everywhere below.

$ cd ~/fastsim

$ git pull

$ catkin build

After this change, you might notice that the visualization is slower. This is expected.

Test Driven Development

In this lab, we use a software development technique, called test driven development. First, test cases are created for the software requirements. The software requirements are then implemented one by one and the test cases executed at each iteration. If the test cases checking the requirement being implemented fail, then the errors must be fixed before moving on to implement a new requirement.

Step 1: Yaw PID Node Requirements

In this lab we will implement a controller to manage the drone's yaw. We start by defining the node requirements.

(R1) Configuration parameters:

The node must read the following parameters from the parameter server: the proportional constant kp, the integral constant ki, the derivative constant kd and the setpoint.

(R2) Inputs:

The yaw_pid_node will subscribe to the topic /uav/sensors/attitude, which contains the values for the three angles roll, pitch and yaw. The third value of the Vector3 message received on the topic is the current yaw measurement.

(R3) Output frequency:

The node must publish on the topic /uav/input/yaw at the rate 100.

(R4) Output value:

The control value calculated using the PID with the constants given by the parameter server assuming the time interval between the yaw measurements is 0.01.

Step 2: Develop Test Cases

Before moving on to developing our own test cases, we must introduce the testing framework available in ROS.

Testing in ROS

In the previous lab, you were introduced to the Python support for unit testing through unittest. We had you execute the tests using python, but the preferred alternative is to integrate the unit tests with the ROS project and use catkin to run them. Thus the tests are a part of the software development process and they are compiled an executed in a manner similar to the compilation and execution of the application. In this lab, you will learn how to use the ROS testing tool called rostest and how to add test files to the build process.

The lowest level of testing is the library unit test, where the functions that do not use the ROS functionality are tested. The test for the MovingAverage methods is an example of a library unit test. This testing is not different from the unit tests you would write for your regular Python code.

The next level of testing is the ROS node unit test. The node unit test starts one node under test and tests external behavior including published topics, subscribed topics and services. In this lab, we focus on node unit tests.

Lastly, the nodes integration and regression testing starts multiple nodes and checks that they are working together as expected.

The last two levels of testing may involve writing additional code for mock nodes that communicate with the nodes under test.

Rostest is an extension to roslaunch that enables the testing analyst to write test cases as roslaunch files.

Install rostest

$ cd ~/fastsim

$ git clone https://github.com/ros/ros_comm.git

$ cd ros_comm

$ git checkout kinetic-devel

$ cd ~/fastsim

$ catkin build

Configure rostest

To signal to catkin that the package simple_control contains tests, we need to make some changes to package.xml and CMakeLists.txt:

package.xml of the simple control package

Add the line below:

<test_depend>rostest</test_depend>

CMakeLists.txt of the simple control package

if (CATKIN_ENABLE_TESTING)

  find_package(rostest REQUIRED)

  add_rostest(tests/params.launch)

  add_rostest(tests/hztest.launch)

  add_rostest(tests/p.launch)

  add_rostest(tests/i.launch)

  add_rostest(tests/d.launch)

endif()

The code above indicates to catkin that if testing is enabled, then the rostest package is required and the tests that are implemented.

Starting Testing Code

We have already developed some tests for you. To get the tests listed above follow the next steps:

$ cd ~/fastsim/src/simple_control

$ mkdir tests

$ cd tests

$ cp ~/fastsim/src/misc/* .

What should we test?

The test driven development requires that a test exist for each requirement of the application. To help you get started, we created a few test cases.

Available Tests

1. for R1 (configuration parameters): test to check that the required ROS parameters are set: params.launch.

This test uses the paramtest functionality available in rostest, specifically paramtest_nonempty. First, the test includes the fly.launch launch file, then it defines the test parameters, which are the ROS parameters that must be non-empty.

<launch>

  <include file="$(find flightcontroller)/launch/fly.launch">

  </include>

  <test pkg="rostest" type="paramtest" name="paramtest_nonempty"

        test-name="paramtest_nonempty">

    <param name="param_name_target" value="/yaw_pid_node/kp" />

    <param name="param_name_target" value="/yaw_pid_node/ki" />

    <param name="param_name_target" value="/yaw_pid_node/kd" />

    <param name="param_name_target" value="/yaw_pid_node/setpoint" />

    <param name="test_duration" value="5.0" />

    <param name="wait_time" value="5.0" />

  </test>

</launch>

2. for R3 (output frequency): test to check that the node yaw_pid_node publishes at the required frequency on the topic /uav/input/yaw: hztest.launch.

This test uses the hztest functionality available in rostest. Here, the node yaw_pid_node is started first, and then the topic messages are checked.

<launch>

  <node name="yaw_pid_node" pkg="simple_control" type="yaw_pid.py">

    <param name="kp" type="double" value="0.0" />

    <param name="ki" type="double" value="0.0" />

    <param name="kd" type="double" value="0.0" />

    <param name="setpoint" type="double" value="0.0" />

  </node>

  <param name="hztest1/topic" value="/uav/input/yaw" />

  <param name="hztest1/hz" value="100.0" />

  <param name="hztest1/hzerror" value="5" />

  <param name="hztest1/test_duration" value="5.0" />

  <test test-name="hztest_test" pkg="rostest" type="hztest" name="hztest1" />

</launch>

The next three tests check that the output values are calculated correctly (R4). For this test, we need to be able to provide the node yaw_pid_node with inputs and read the corresponding output, and then check them against the expected values. Thus, we need a mock test node that implements the three functionalities described:

1. sends the proper inputs to the yaw_pid_node, that is Vector3 instances on the topic /uav/sensors/attitude, with the third component the yaw defined by the test

2. reads the values published by the node on the topic /uav/input/yaw.

3. compares the output of the node with the expected values.

The mock test node we provide reads a space separated yaw measurements from the parameter yaw, and the expected values from the ROS parameter expected. The test node waits until at least one node subscribes to the topic /uav/sensors/attitude, then it sends the values from the ROS parameter yaw. The test node then waits to receive a number of distinct values different than 0.0 on the topic /uav/input/yaw. After the values are received, then they are compared against the values from the ROS param expected.

Once the mock test node is implemented, the tests cases are defined in separate ROS launch files which start launches two : the yaw_pid_node with the ROS params included, and the test node test_yaw_pid.

3. (R4) output: test to check the proportional controller: p.launch.

To test the proportional controller, we set the kp parameter of the yaw_pid_node to a nonzero value (3.0 in the test case below), and the other constants to zero.

Next, we have to define the inputs in the ROS parameter expected, 1.0 and 0.5 in this case. The values in the yaw parameter are chosen to make the calculations easier.

Finally, we have to manually calculate the expected outputs based on the requirement that the node under test implement the PID controller. We use the formulas described in the beginning of the lab to calculate the control signal, y.

The setpoint is 0.0, thus:

The output variables are given in the test in the yaw ROS parameter:

The error terms are:

The control variable values are:

In conclusion, the expected ROS parameter is the list of the control variables: -3.0 -1.5.

Here is the complete test case that starts two nodes: the yaw_pid_node which implements the yaw PID controller, and test_yaw_pid, which is the mock test node.

<launch>

  <node name="yaw_pid_node" pkg="simple_control" type="yaw_pid.py">

    <param name="kp" type="double" value="3.0" />

    <param name="ki" type="double" value="0.0" />

    <param name="kd" type="double" value="0.0" />

    <param name="setpoint" type="double" value="0.0" />

  </node>

  <param name="yaw" type="str" value="1.0 0.5" />

  <param name="expected" type="str" value="-3.0 -1.5" />

  <test test-name="test_yaw_pid"

   pkg="simple_control"

   type="test_pid.py"

   time-limit="10.0"

   name="p" />

</launch>

Note: The time-limit parameter specifies the maximum time interval for which the test is allowed to run.

We provide similar tests for the the integral controller: i.launch , and the derivative controller: d.launch. For i.launch, calculate the expected output by yourself for proportional and derivative constants equal to zero, setpoint equal to zero and the yaw values 1.0 and 0.5. Do the same for d.launch, but use the derivative constant equal to 3.0 and the others zero.

Use the commands below to inspect the test cases check that your calculations are the same as in the test:

$ cat ~/fastsim/src/simple_control/tests/i.launch

$ cat ~/fastsim/src/simple_control/tests/d.launch

New Tests

1. Add three new tests of your choice in the test folder.

To decide what tests to implement read the requirements and find aspects or parts of the requirements not tested yet. For example, the (R2) requirement is not tested separately. It is implicitly tested in the last three test cases, but we don't have a test for it by itself. For (R4), we tested the PID components only for one or two values, and only one component of the sum at a time. Other test cases might use a combination of values and longer inputs.

2. Make the necessary changes to the CMakeLists.txt to add the three new tests you just created.

Step 3: Run the test cases

The next step in the test driven development is to create a skeleton implementation of the node and run the test cases. Since we didn't actually implement the tested functionality, the test cases must fail. If there are test cases that pass, then there is an error in the test cases design or the testing framework implementation that must be addressed before moving on to the implementation on the node under test.

First, we need to create a yaw_pid_node node that does nothing and then run the test cases.

Yaw PID Node

We start by creating the yaw_pid_node with an empty implementation. The subscriber callback sets an instance variable with the yaw reading. The control_loop calculates the control output using the last reading received. The self.changed attribute is used to process only when a new value was received.

Create a node yaw_pid_node in the package simple_control in the file yaw_pid.py. Copy the skeleton code below, but do not implement it yet.

Notes:

1. The TODOs are out of order because they implement different requirements and the logical order of the requirements does not match exactly the code layout.

2. There is a TODO without a number. This is to signal that you may need to initialize some variables used in the loop.

Observation:

1. You may use the following command to change the setpoint parameter while the application is running. It is helpful to use it during tuning if you want to see how your controller behaves with different setpoints. Simply change the 0 value to the yaw you want.

rosparam set /yaw_pid_node/setpoint 0

2. Avoid using yaw angles close to pi and negative pi.

Code:

#!/usr/bin/env python

import rospy

from threading import Lock

from geometry_msgs.msg import Vector3

from std_msgs.msg import Float64

# Please implement the TODOs after you write the test cases.

class YawPID():

    def __init__(self):

        # this is used to check that a new measurement was received

        # the callback should set it to True

        self.changed = False

        self.output = 0.0

        self.lock = Lock()

        # TODO (3) read param /yaw_pid_node/kp

        # TODO (4) read param /yaw_pid_node/ki

        # TODO (5) read param /yaw_pid_node/kd

        # TODO (3) create subscriber to the topic '/uav/sensors/attitude' with type Vector3

        # TODO (2) create publisher on the topic '/uav/input/yaw' with type Float64

        self.control_loop()

    def yaw_angle_callback(self, msg):

        self.lock.acquire()

        # TODO (3) update the changed flag

        # TODO (3) record new measurement

        self.lock.release()

    def control_loop(self):

        # TODO initializations for any variables you may use in the control loop

        # TODO (2) set the rate (use the variable ros_rate)

        while not rospy.is_shutdown():

            # TODO (3) read param /yaw_pid_node/setpoint

            self.lock.acquire()

            if self.changed:

                self.changed = False

                output = 0.0

                # TODO (3) calculate error

                # TODO (3) reset self.output

                # TODO (3) add the proportional term to self.output

                # TODO (4) add the integral term to self.output

                # TODO (5) add the derivative term to self.output

            self.lock.release()

            # TODO (2) publish the control self.output

            ros_rate.sleep()

if __name__ == '__main__':

    rospy.init_node('yaw_pid_node')

    try:

        YawPID()

    except rospy.ROSInterruptException:

        pass

Run the Test Cases

To run all the test cases in an workspace, we use the command:

$ catkin run_tests

This command uses the catkin configuration files to find all the test cases in the workspace and execute them. Since our workspaces includes many other packages and we are interested only in node testing of the yaw_pid_node, we execute the commands below:

$ cd ~fastsim/src/simple_control/

$ catkin run_tests --no-deps --this --summarize

The option --this tells catkin to executed only the tests of this package from which it is invoked, simple_control in this case. The --no-deps option tells catkin not to compile and test dependencies.

The test status is displayed to the standard output, as well as in the following folder: ~/fastsim/build/simple_control/test_results/simple_control/.

Parsing the console output might be rather hard. The status of each test case and the output can be found in an xml file (one for each test case) in the directory ~/fastsim/build/simple_control/test_results/simple_control/. To quickly check that your test cases passed run the command below:

grep "results summary" ~/fastsim/build/simple_control/test_results/simple_control/*

For each test case you should see a line like the one below:

>>>>> rostest-tests_d.xml:[ROSTEST]test [test_yaw_pid] results summary: 0 errors, 1 failures, 1 tests

Checkpoint 1

1. What tests did you add? Why?

2. Show the output of running the grep command.

Remember, the tests are expected to fail at this point in the lab because, the PID controller is not implemented.

3. Why do you think there may be issues for an yaw close to pi?

Implementing the PID Controller

For the implementation of the PID controller we proceed with writing code incrementally and running the relevant test cases after each requirement is implemented.

In this section, we execute the test cases one by one, using the rostest command.

1. Add the node and the ROS parameters. (R1)

Add the node to the fly.launch file:

<node name="yaw_pid_node" pkg="simple_control" type="yaw_pid.py">

    <param name="kp" type="double" value="0.0" />

    <param name="ki" type="double" value="0.0" />

    <param name="kd" type="double" value="0.0" />

    <param name="setpoint" type="double" value="0.0" />

</node>

Build and source the workspace again and run the test command:

$ rostest src/simple_control/tests/params.launch

Now, the output should be:

SUMMARY

 * RESULT: SUCCESS

 * TESTS: 1

 * ERRORS: 0

 * FAILURES: 0

2. Implement the code for publishing on the topic /uav/input/yaw. Use the test hztest.launch to check your implementation. (R3)

Run the command below:

$ rostest src/simple_control/tests/hztest.launch

The test is expected to fail.

Add the code for initializing the publisher in __init__, the loop that publishes at the required rate (the TODO #2).

.....

>>>> [simple_control.rosunit-hztest_test/test_hz][FAILURE]---------------------------

>>>> no messages before timeout

.....

Implement the code until the test passes or you have the error:

average rate (44.906Hz) exceeded minimum (45.000Hz)

There are two possible causes for this error: (1) the rate for the PID loop is incorrect and in this case you should fix it, or (2) your computer is simply not powerful enough to publish at the required rate, in this case you need to change the test parameters. To be able to continue, you may edit the hztest.launch file to increase the error margin by editing the parameter below:

  <param name="hztest1/hzerror" value="5" />

3. Implement the proportional controller, TODO #3. Use the test p.launch to check your implementation. (R1, R2, and R4)

4. Implement the integral controller, TODO #4. Use the test p.launch to check your implementation.

5. Implement the derivative controller, TODO #4. Use the test p.launch to check your implementation.

6. Run the tests you added and fix errors until they pass.

Enable the Yaw Controller

Important: The simulator already has a yaw controller implemented. In order to have yours running you must make the changes below.

In the file ~/fastsim/src/flightcontroller/launch/angle.launch change the parameter no_yaw to True. That is the line:

 <param name="no_yaw" type="bool" value="False" />

should be:

 <param name="no_yaw" type="bool" value="True" />

After you make this change, the drone does not hover steadily anymore. Only after you successfully tuned your PID controller, the drone will be able to change the yaw and hover steadily.

Tuning the PID controller

Now that you have an working implementation of the PID controller, it is time to find a set of constants for the PID controller that move the drone from the default value to the yaw/setpoint ROS param defined in fly.launch. Specifically, you have to change the values of the following ROS params: yaw/kp, yaw/ki, yaw/kd.

Record the values you try.

You may use the following strategy:

1. Start with all the constants set to 0.0

2. Increase kp until the drone oscillates steadily.

3. Increase kd until the oscillations stop.

4. Repeat steps 2 and 3 until increasing kd does not stop the oscillations anymore.

5. Set kp and kd to the last stable values.

6. Increase ki until the drone rotates to the yaw given by the setpoint.

Checkpoint 2

1. What values did you try for kp, ki, and kd?

2. Launch the drone and take a series of screenshots of the drone reaching the desired yaw.

Congratulations, you are done with lab 6!

To Check

Checkpoint 1:

1. What tests did you add? Why?

2. Show the output of running the grep command.

Checkpoint 2:

1. What values did you try for kp, ki, and kd?

2. Launch the drone and take a screenshot of the drone reaching the desired yaw.

Extra

For a brief introduction on drone control, watch the video below.