Week 3

Creating a ROS Node

ros::init(argc, argv, "NODE_NAME")

Writing A ROS Publisher

4 steps:

1. Need a ros::NodeHandle

ros:NodeHandle nh;

2. Need a ros::Publisher

ros::Publisher publisher = nh.advertise<MESSAGE_TYPE>(TOPIC, QUEUE_SIZE)

3. Create the message

std_msgs::Int32 msg;

4. Publish the message

publish.publish(message)

Writing A ROS Subscriber

4 steps:

ros:NodeHandle nh;

void callbackFunction(std_msgs::Int32 msg) {
  // Stuff
}

4. ros::spin()

ros::spin()

Example

• Task: stop a car in front of the stop sign

How do you do that?

• Gauge the distance, or error, between current position and where you're trying to stop
• Use that information to decide how much throttle

• Controller determines how much throttle
• Goal: Process variable (PV) equal to set point (SP) by changing the control

Back to car example

• Process Variable (PV): Car's position
• Set Point (SP): Position right in front of the stop sign
• Control: Throttle

Intuition:

• Larger error → Step on throttle more
• Smaller error → Step on throttle less
• No error → Don't step on throttle

Proportional controller

control is proportional to the error

Launching buzzsim with roslaunch

• This time we'll use roslaunch to launch the simulator

  roslaunch igvc_training_exercises week3.launch

• Syntax quite similar to rosrun

  • roslaunch PACKAGE_NAME LAUNCH_FILE

• Instead of buzzsim, there's a week3.launch

• Two <node> tags
  • Specifies a node to launch
  • buzzsim from igvc_buzzsim package
  • week3 from igvc_training_exercises package

• Three <param> tags
  • One is commented out with <!-- -->
• We'll get to these <param> tags later

• What does a .launch file do?

Lets you define nodes to launch and their respective parameters.

The buzzsim environment for the exercise

Goal: Move the bottom turtle to the top turtle using PID

• /oswin/velocity
  • Send velocity commands to the bottom turtle
• /oswin/ground_truth
  • Get where the bottom turtle is
• /kyle/ground_truth
  • Get where the top turtle is

How?

1. Create a subscriber to the ground truth topics
2. Create a publisher to the velocity topic
3. Save Kyle's pose in a global variable
4. In the callback for

   /oswin/ground_truth

   , calculate the error
5. Multiply the error by

   Kp

   to calculate the velocity
6. Publish the velocity to the velocity topic

Subscribe to the ground truth topics for both turtles

Create a subscriber for /oswin/ground_truth and /kyle/ground_truth to find where the turtles are

• Make sure to #include the message type!
  • Clion should yell at you if you don't. Listen to Clion.

Save the position in a global variable

Save the position for /kyle/ground_truth in global variable g_kyle_pose

Sanity check - everything is still working

How do I check to make sure the subscribers are working?

ROS_INFO_STREAM

Publish to the /oswin/velocity topic to control the bottom turtle

• Create a publisher for the /oswin/velocity
• Put the publisher in the global variable g_velocity_pub
  • So that we can access it in the callback

Calculate the error in position

Calculate the error in x-coordinate between the two turtles

Implement a proportional controller in the callback for /oswin/ground_truth

1. Create a variable kp to store the coefficient.
2. Calculate
3. Publish to the publisher created earlier

With kp = 1, after you compile and roslaunch you should then see the bottom turtle move up and stop exactly where top turtle is.

What happens with different values of kp?

• ros::NodeHandle has a .param(PARAM_NAME, VARIABLE) method you can call
  • Give you the value of a parameter

To make kp a parameter:

double g_kp;
...
int main(int argc, char** argv)
{
  ros::init(argc, argv, "week3");

  ros::NodeHandle private_nh{"~"};
  pnh.getParam("kp", g_kp);
  ROS_INFO_STREAM("kp: " << g_kp);
}

• We pass "~" as a parameter to the ros::NodeHandle constructor
  • Makes ros::NodeHandle refer to the node's private namespace
  • Namespace between the <node> tags in a .launch file.

Also need to set the parameters in the .launch file

• Add a <param> tag between the <node> tag that launches the week3 node:

<node pkg="igvc_training_exercises" type="week3" name="week3" output="screen">
    <param name="kp" value="1.0" />
</node>

• Correct value of the kp parameter is printed
• Can change the value in the week3.launch without recompiling

• Create a publisher of type std_msgs::Float64 with the topic "error"
• Publish the "error" variable calculated in the callback for /oswin/ground_truth

How can you verify that it is publishing correctly?

rostopic echo /error

• ROS comes with the tool rqt_plot for plotting graphs and visualizing data

  rqt_plot

• Add the /error topic to the graph
  • Type /error at the top

Launch the simulation and the controller. You should see a graph like below:

• Try playing with kp again
  • Verify that the graph shows what you see happening in the simulator.

• Intuition:
  • If error wasn't decreasing, step harder on the throttle
  • The longer the error is present, the harder you step on the throttle.

• Can describe this behavior with the integral of the error
  • If the error is present for longer, the integral of the error will increase
  • Increase our control effort

This is an integral controller, where the control is proportional to the integral of the error

• Integrals are hard
• Easier to think of this in terms of discrete time
  • time happens in small steps
• Instead of computing an integral, we compute a sum of the error
  • Multiply it by the time step between each interval

or calculating recursively

It should look like this:

<launch>
    <node pkg="igvc_buzzsim" type="buzzsim" name="buzzsim">
        <param name="config_path" value="$(find igvc_training_exercises)/config/week3/world.yml" />
<!--        <param name="world_name" value="stationary" />-->
        <param name="world_name" value="moving" />
<!--        <param name="world_name" value="circle" />-->
    </node>

    <node pkg="igvc_training_exercises" type="week3" name="week3" output="screen">
        <param name="kp" value="1.0" />
    </node>
</launch>

• If you roslaunch again, you should see the top turtle start to move
• Does your proportional controller work?
• Verify what you're seeing by checking rqt_plot.

The error that you're seeing is called steady state error. As we explained earlier, the integral controller helps to solve this.

The PID Algorithm - Derivative Controller

• Imagine we're back in the first scenario where we're trying to stop right in from the stop sign
• We're driving a massive truck
• What problems does our proportional controller have?
  • We keep our foot on the throttle even when we're going full speed in front of the stop sign
  • Result in overshoot, oscillating back and forth

• Intuition:
  • Step on the throttle at the beginning to get the truck moving
  • As the truck begins to accelerate, use the throttle less
  • The faster you're approaching the target position, the more you ease off the throttle, or even apply the brakes

Another example:

• Scenario: opening a valve to fill a cup of water
• If the cup is filling up too quickly, close the tap to prevent water from overflowing
• On the other hand, if the cup broke a hole opened up at the bottom and water started to leak
  • Since error is now increasing quickly, the derivative term will try to open up the tap more

• Component of control that is proportional to the derivative of the error
  • The faster the error is decreasing, the more we ease off on our control
  • The faster the error is increasing, the more we increase our control

This is a derivative controller, where the control is proportional to the derivative of the error.

For the discrete time version, calculate the derivative with finite differences:

Exercise: Not overshooting, and derivative

• This is left as an exercise for the reader again

• Similar to last week, check out the markdown version on github
  • Contains additional exercises (implementing the I and D parts of PID)
• Any questions?

• PID control and basic control theory
  • Controlling a Process Variable to equal some Set Point
  • The Proportional Integral Derivative algorithm as a simple controller
  • Understanding what a controller does in the context of a car

• roslaunch and .launch files
  • Using roslaunch <ros package> <launch file> to launch a .roslaunch file
  • <node> tags to define ROS nodes to be launched in a .roslaunch file

• ROS parameters
  • Using nh.getParam to get parameters defined in a launch file
  • Adding parameters to the launch file with the <launch> tag

• Written our own PID controller
  • Implementing something we just learnt about in theory!
  • And it works!

Next Week

We'll learn about reference frames, the IMU, and localization with dead reckoning by integrating IMU data.