Week 5

IMU

What is an IMU?

Inertial Measurement Unit Measures acceleration, angular velocity, and heading

Coordinate Frames

What is a coordinate frame?

Defines a set of directions relative to some object

Localization

What is localization?

Finding where you are

What is one way to do localization?

Dead Reckoning

Dead Reckoning

How does dead reckoning work?

Integrate accelerations and velocities to obtain position with kinematics

Visualization

How can we visualize things in ROS? (ie. Localization node)

rviz

• Take distance measurements around the robot
• Allow the robot to "see" obstacles
  • 2D or 3D

Notice that we are able to "see" the surrounding obstacles from the "shape" of the lidar scan. We can use this to construct a map of our surroundings.

The simulation should pop up with a lidar visualization like the above image

Open up rviz:

rviz

• Change the fixed frame in the top left corner of the left menu to oswin

• Set the target frame in the right menu to oswin as well.

• Add the "PointCloud2" visualization so you can visualize the lidar data

  • Click add in the left menu, select "PointCloud2"
  • Change the "Topic" to /oswin/pointcloud.

In a third window, open up teleop_twist_keyboard as you have done many times already:

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

Grid Maps

• How do you do mapping?
• One way is through the use of grid maps

• grid map uses a grid to represent the world
• The locations of any obstacles are then discretized into the grid cells.

Example:

• Dimension: 5m x 10m
• Resolution: of 10cm per grid cell

How many total grid cells?

(5m * 10 cells/m) * (10m * 10 cells/m) = 5000

Ex:

• (0, 0) in the map corresponds to (0m, 0m) in the real world
• Obstacle (let's assume it's a single point) located at coordinates (2m, 7m).

What grid coordinates would this translate to?

(2m * 10 cells/m, 7 * 10 cells/m) = (20, 70) on the grid

Suppose there's another obstacle (also a single point) located at coordinates (2.01m, 7.01m). What grid coordinates would that translate to?

(2.01m * 10 cells/m, 7.01 * 10 cells/m) = (20, 70) on the grid

• This is one of the disadvantages of using grid maps
• Discretization error, depending on the resolution of the map.

• Advantage: easy to work with in terms of mapping as well as path planning
  • We will get into this next week

• Data from the lidar is obtained relative to the sensor position
• We want this data in the global coordinate frame, one of which is the "odom" frame
  • REP 105

We need to transform these coordinates from the sensor's coordinate frame to the "odom" frame

• Assume our robot is at the coordinates (1,2) facing 90 degrees
  • x points East and y points North
  • with 0 degrees yaw being East and increasing CCW
• Lidar at same location sees an object at coordinates (3, -1) in the robot's frame
  • Body frame's x points forward and y points left
• What are the coordinates of (3, -1) in the odom frame?

• (3, -1) in the odom frame has the coordinates of (2, 5) (Red is the x-axis, Green is the y-axis).
• Once we have transformed this lidar point into the odom frame, we can then insert this point into our map
  • Mark it as occupied

Mapping: repeat this procedure for every point in the lidar scan every time we get a lidar scan.

• Multiple ways of obtaining the transforms
  • Look up using tf, a useful package that makes coordinate transforms easy in ROS.

Coordinate transforms with tf::TransformListener

• Need to use something called a tf::TransformListener

• "listens" for coordinate transforms between different reference frames

• listen for transforms broadcasted by tf::TransformBroadcaster in other nodes

We don't need to worry about the tf::TransformBroadcaster part as the simulation already takes care of that for us

To use a tf::TransformListener, first create one like so:

tf::TransformListener listener;

Then, to use it to lookup transforms, say from the frame "oswin" to the frame "odom", do the following:

tf::StampedTransform transform;
try {
  listener.lookupTransform("odom", "oswin", msg.header.stamp, transform);
} catch (tf::TransformException &ex) {
  ROS_ERROR_STREAM(ex.what());
}

lookupTransform (const std::string &target_frame,
                 const std::string &source_frame,
                 const ros::Time &time,
                 StampedTransform &transform) const 

• Pass in msg.header.stamp to specify that we want the transform at that moment
• Can also pass ros::Time(0) to signify we want the latest transform.

• Notice that we use a try { ... } catch () { ... } statement here
• This is because listener.lookupTransform() might throw an exception if some error occurs
  • Unable to find the transform requested
• If we don't add a try { ... } catch() { .. } here, then the program will crash when an exception is thrown

Common snippet:

tf::StampedTransform transform;
ros::Time stamp = msg.header.stamp;
if (!listener.canTransform("odom", "oswin", msg.header.stamp)
{
    stamp = ros::Time(0);
}
try {
  listener.lookupTransform("odom", "oswin", msg.header.stamp, transform);
} catch (tf::TransformException &ex) {
  ROS_ERROR_STREAM(ex.what());
}

• The details are kind of confusing for ROS
  • There have been a few different versions
  • We'll use the type pcl::PointCloud<pcl::PointXYZ>

• Notice that pcl::PointCloud is a template as it uses angle brackets
• Don't worry about the template part of this though
  • Basically only use pcl::PointXYZ.

To transform a pointcloud, we can make use of a function from pcl_ros:

pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
pcl_ros::transformPointCloud(input_cloud, transformed_cloud, transform);

• input_cloud is the pointcloud you want to transform
• transform is the tf::Transform representing the transformation

Some important points:

• The info member contains information about the map: width, height etc.

• The data member (of type std::vector<int8_t> needs to be initialized

  • In main before you call ros::spin() make sure that the data member is initialized to have as many elements as there are grid cells, ie.

    map.data = std::vector<int8_t>(total_cells, 0);

• The map is 2D, but the data member is only 1D. This means that you'll need to convert from 2D coordinates to an index.

  • This works by imagining an index that increments left to right, top to bottom. For example, for a 3x3 grid:

    0 1 2

    3 4 5

    6 7 8

1. Subscribe to /oswin/pointcloud and write a callback function taking in pcl::PointCloud<pcl::PointXYZ>
2. Create a global tf::TransformListener and call lookupTransform from oswin to odom inside the callback
3. Transform the pointcloud from the oswin frame to the odom frame using pcl_ros::transformPointCloud
4. Iterate through the pointcloud. For each point, find which grid cell it is, then increment that cell by 1 if its less than 100
5. Create a publisher of type nav_msgs::OccupancyGrid and publish the map

• Open up rviz and add the nav_msgs::OccupancyGrid visualization
• See a map that gets populated with obstacles as you drive around in the simulation with

• The LiDAR
  • Takes distance measurements around the robot
  • Allows the robot to "see" obstacles

• Grid Maps
  • A grid map discretizes the world into cells
  • Easy to implement and use, but may have discretization errors

• Coordinate Frames and Transforms
  • The coordinate of objects in different coordinate frames is different
  • We can use transforms to transform the coordinates of an object from one frame to another

• Transforming Pointclouds in ROS
  • Use the pcl_ros::transformPointCloud function to transform pointclouds to a different coordinate frame

• Implementing a simple mapper
  • Getting practice with working with pointclouds and performing coordinate transforms