Week 4

Control Theory

control a system to do what we want

What's one example of a controller that we learned last week?

PID

What does PID stand for?

Proportional Integral Derivative

What is a proportional controller?

A controller where the control effort is proportional to the error

What is a integral controller?

A controller where the control effort is proportional to the integral of the error

Why do we need an integral controller?

A proportional controller cannot deal with steady state error.

What is a derivative controller?

A controller where the control effort is proportional to the derivative of the error

Why do we need a derivative controller?

To reduce overshoot from the controller

How is roslaunch different from rosrun?

roslaunch

roslaunch

How do you define a parameter?

nh.getParam(param_name, variable)

<param name="param_name" value="value" />

How would you visualize messages on a graph?

rqt_plot

• Sensor used for finding orientation
• A 9 DOF (degree of freedom) IMU measures 9 things
  • Accelerations (x, y, z) with acceleromteter
  • Angular velocity (x, y, z) with gyroscope
  • Magnetic Field (x, y, z) with magnetometer

Find acceleration, angular velocity and heading

A coordinate frame refers to the coordinate system that is used

• For example, the IMU can measure accelerations in the x, y and z axes in the robot's coordinate frame
• This is different from the world's coordinate frame.

An example:

• IMU is mounted in the same direction as my robot
• When the IMU records an acceleration in the X direction, it means that the robot has accelerated forwards in the +X direction (cyan arrow).

• Robot is located at (2,1), facing north
• Robot's coordinate frame
  • +X acceleration
  • (1, 0, 0)
• World's coordinate frame
  • +Y acceleration
  • (0, 1, 0)

Note

ROS has a REP 103 standard

• Defines conventions for coordinate frames
• Right hand coordinate frame
• For bodies (robobts)
  • X points forward, Y points left, and Z is pointing up
• For geographic locations
  • X points east, Y points north, and Z points up.

Localization with dead reckoning

• In robotics, we need to figure out where we are
• Called localization

• Can perform localization with an IMU
• Simplest form of localization is called dead reckoning

Dead Reckoning

• Determine position by taking your previous position and integrating your velocity over the elapsed time
• For IMU, linear accelerations instead of linear velocities
  • Need to integrate twice
  • Keep track of our velocities.

This means that our state, the things we need to keep track of, consists 4 things

• x
• y
• heading
• x velocity.

Why only x velocity? What about y velocity?

Because our robot is a differential drive, we can't move sideways, so y velocity is zero

Which coordinate frame is the x velocity in?

The robot's frame

How do we use the IMU measurements to update our state?

Reminder: IMU gives us x acceleration, angular velocity, heading

Hint: Physics

Use kinematics equations:

• Many ways of numerically integrating the data that yield more accurate results
• This is just one simple way.
• By doing this, we should be able to keep track of our position.

Let's look at what topics are published. How do I find that out?

rostopic list

• There's now a topic /oswin/imu of type sensor_msgs/Imu.
• Also, you can see that /oswin/ground_truth isn't there
• We'll need to use dead reckoning to compute an estimate of our pose.

Let's graph /oswin/imu and see what we get

• Open up rqt_plot and graph the accelerations in the x axis
• Open up teleop_twist_keyboard as before:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/oswin/velocity

Move around a bit and get a feel for what the accelerations from the IMU look like as you move around.

Its time to start writing some code. We'll be implementing this node in week4/main.cpp.

What's our plan for writing this node?

1. Create a subscriber to /oswin/imu
2. Create a publisher to /oswin/odometry
3. Implement dead reckoning in the callback
4. Publish the estimated position

1 & 2: Create the publisher and subscriber

ROS_INFO_STREAM the x acceleration to make sure everything is working.

3. Implement Dead Reckoning

• Before we start, we need to setup the global variables representing our robot state
  • x, y, x velocity, heading
• Also need to keep track of the time which we received the last message
  • To calculate the delta t for the kinematics

Kinematics

• Publish this information so that we can visualize it, and so that other nodes are able to use this information
• ROS provides the nav_msgs::Odometry message type that describes the robot's current location.

What is a Quaternion?

Representing orientations: euler angles and quaternions

• The orientation field uses something called quaternions
• Use 4D unit vector to represent orientations in 3D space
• If you're interested in the math of how quaternions work, feel free to google
  • There's a neat video by 3Blue1Brown that explains it.

• Don't worry if you don't understand how they work though
• We can work with another representation of orientations called euler angles which are much simpler to understand

Euler Angles uses three numbers: roll, pitch and yaw

If you're familiar with airplanes then this should be familiar to you

• roll represents rotation around the x axis
• pitch represents rotation around the y axis
• yaw represents rotation around the z axis.
• Right hand rule

• Because we're working in 2D, roll and pitch are 0
• Only need to worry about is yaw.

Converting between euler angles and quaternions

In order to convert between euler angles and quaternions, the ROS built-in tf package provides a few useful methods that we can use.

Add the following #include:

#include <tf/transform_datatypes.h>

We can then use the tf::createQuaternionMsgFromYaw message to convert from yaw to a quaternion. This might look something like:

odometry_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(g_heading);

The Header message type

• One important field that we haven't gone over yet is the Header field on nav_msgs::Odometry
• If you've finished the Integral and Derivative controller from last week, then you probably have seen this before

Let's look at the definition of the message from the ros documentation

• stamp refers to the timestamp of the message.
• frame_id on the other hand refers to the name of the coordinate frame of the message

• x acceleration is different depending on which coordinate frame you're talking about
• Use frame_id to be clear about which coordinate frame we are talking about

If our world frame was called yeet, and we wanted to say that the position of our robot was in relation to the yeet frame

nav_msgs::Odometry msg;
msg.header.frame_id = "yeet";

Of course, don't actually call your coordinate frame yeet

• ROS defines a few standard coordinate frame names in REP 105
• We will be using the odom frame, which is a world-fixed frame:

nav_msgs::Odometry msg;
msg.header.frame_id = "odom";

Open up rviz by running the rviz command:

rviz

You should see something similar to the screen below:

In order to visualize the nav_msgs::Odometry message that we're publishing:

1. On the left in the Global Options section change the Fixed Frame to "odom", the value of the frame_id we set earlier.
2. On the right, in the Views section, change the Type to TopDownOrtho to change the view from 3D to top down 2D.
3. Click the Add button on the bottom left, select the By topic tab, and click Odometry under /oswin/odometry.

You should now see a red arrow in the middle of your screen visualizing the nav_msgs::Odometry message that you're publishing.

Try moving the robot again using teleop_twist_keyboard

• Both the turtle in the simulator and the red arrow move
• Hopefully the red arrows will be in the same location as the turtle in the simulator

tf and tf::TransformBroadcaster

We're almost done

The last thing we need to do is to "broadcast" our transform (the position of our robot) to tf

• tf needs to know where these coordinate frames are in
• We can do that by broadcasting the transform
• To do that, we need to do three things:

1. Create a tf::TransformBroadcaster

Create a tf::TransformBroadcaster:

tf::TransformBroadcaster transform_broadcaster;

2. Create a tf::TransformStamped to hold the transform

• We need to create a tf::TransformStamped variable in order to pass it to the tf::TransformBroadcaster that we just made
• Unfortunately, since this isn't a ROS message, this can't be looked up as easily as other messages

Thankfully, Clion is here to save the day

• Type in tf::TransformStamped, and then move your cursor over it
• Hit Ctrl-Alt-B (Or Ctrl-Shift-A and search "go to defintion") to go to the definition of tf::TransformStamped
• Find the constructor (to see what fields we can pass it):

StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id,
                 const std::string & child_frame_id):
    tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id),
    child_frame_id_(child_frame_id)
{ };

We need to pass 4 things:

1. tf::Transform input
2. ros::Time timestamp
3. std::string frame_id
4. std::string child_frame

tf::Transform represented the actual transform between the two coordinate frames

Two useful methods that we need:

.setOrigin(tf::Vector3 origin)

• takes in an argument of type tf::Vector3
• Sets the origin, or position of the transform
• Don't worry, tf::Vector3 has a sane constructor - you pass in x, y and z coordinates in the constructor:

tf::Transform transform;
tf::Vector3 origin(1.0, 2.0, 3.0);
transform.setOrigin(origin);

.setRotation(tf::Quaternion quaternion)

• Takes in an argument of type tf::Quaternion
• Sets the rotation of the transform

But theres one problem:

• Oh noes, its in quaternions and not yaw!
• We can just do the same thing as we did earlier
  • tf::createQuaternionFromYaw instead of tf::createrQuaternionMsgFromYaw

tf::Transform transform;
transform.setRotation(tf::createQuaternionFromYaw(1.57));

Next, ros::Time timestamp

• Time at which this transform was recorded

Why does there need to be a timestamp?

• For static transforms, ex. transform from the imu to your robot
  • the timestamp doesn't matter
• For other transforms, ex. transform from the world frame to your robot AKA your robot's position
  • transform is different at different moments in time
• We'll make use of this next week.
• We can just use the timestamp of the IMU message that we received.

Finally, the frame_id and child_frame arguments of type std::string

• Names of the frame and child frame for the transform you're trying to publish

For example, trying to publish the transform of the robot frame in the world frame

• frame_id (the reference frame we're currently in) is odom
• child_frame (the reference frame we're trying to describe) is oswin.

3. Call the .sendTransform function on the tf::TransformBroadcaster

Finally, we call the .sendTransform function on the tf::TransformBroadcaster we created earlier.

That's it....... except not really. Most likely you've created tf::TransformBroadcaster in one of two ways:

1. Created it as a global variable
2. Created it in the callback

Both of these ways don't work

We can verify this by running rosrun tf tf_monitor, a tool which prints out all the coordinate frame transforms that tf receives

If it were working, you would see something like:

Frame: oswin published by unknown_publisher Average Delay: 0.000244823 Max Delay: 0.000300968

But it's empty. What's the problem?

If you created it as a global variable

• You must call ros::init() before creating the first NodeHandle
  • The constructor of tf::TransformBroadcaster creates its own ros::NodeHandle
  • This has to happen after ros::init()

If you created it in the callback, no error shows, but it still doesn't work.

• Problem: tf::TransformBroadcaster needs some time to send the transform
  • Similar to ros::Publisher during the first week
  • We can't wait here though, since we need the transforms as soon as possible
  • We want to know where we are right now, not like 1 second ago

Want to be able to create the tf::TransformBroadcaster after we do ros::init

Only create it once and not everytime

How can we fix this problem?

We can do this with static local variables

So, if we created tf::TransformBroadcaster as a static variable inside the callback like so:

void imuCallback(sensor_msgs::Imu msg)
{
  ...
  static tf::TransformBroadcaster transform_broadcaster;
}

• First time the callback is called, which is after we do ros::init, the tf::TransformBroadcaster variable will be initialized
• Next time the callback is called, the same instance of the variable will still be there
  • Enough time to send the transform

You should see that rosrun tf tf_monitor is able to show that the oswin frame is getting published to by your node

Visualizing tf frames in rviz

• We can visualize the tf frame we published to in rviz
• Open up rviz, and then add a display of type tf.
• You should be able to see two frames show up on the screen: "odom" and "oswin"

• Verify that tf::TransformBroadcaster is working properly by moving the turtle around
• The "oswin" frame should also be moving.

Summary

And that's it for this week!

• The IMU
  • Measures acceleration, angular velocity, and orientation
  • Has an accelerometer, gyroscope and magnetomer

• Coordinate Frames
  • Coordinate Frames defines a set of direction relative to some object
  • Understanding the basics of the REP 103 standards for Coordinate Frames

• Localization with Dead Reckoning
  • Using kinematics equations to locate ourselves

• Implemented dead reckoning in ROS
  • nav_msgs::Odometry message type for odometry information
  • Turned something theoretical to something that was working again!

• rviz
  • Use rviz to visualize what's happening