Homework 6: Vehicle Pose

General Overview

DUE DATE: TBD
POINTS: 30
ROS 2 Topics: vehicle_pose (pub), gps (sub) and odometry (sub)
ROS 2 Messages : Odometry in nav_msgs, NavSatFix in sensor_msgs PoseStamped in geometry_msgs

Deliverables

gps_data.txt and angle_data.txt from Task 2
Google Earth image from Task 2
pose_estimator node python file from Task 3

Tasks

Task 1: Data Collection

Place your vehicle in joystick control. On the vehicle, launch the file vehicle.launch.py so that the nodes gps_node and motor_carrier_driver node are both running.
Run your vehicle outdoors, away from trees (as much as possible). Echo the topics gps (NavSatFix message) and odometry (Odometry message) to make sure that data is being published.
Run for about 1 minute and bag all data. (ros2 bag record -a)

Task 2: Plot latitude and longitude on Google Earth

Write a node that you will run when you play back your bag data. This node will subscribe to the gps topic and the odometry topic.
- Each time a gps topic is read:
  write the latitude and longitude to a text file named lat_lon_data.txt. Put a comma between the latitude and longitude values.
  
  Also, write the UTM position coordinates to a file named gps_data.txt. Put a comma between the values.
- Each time an odometry topic is read:
  write the pose.pose.orientation x,y,z,w values and the calculated orientation angle to a text file named angle_data.txt.
  
  Recall that for our vehicle operating in the x,y plane,
  
  \[\theta = 2*atan2(z,w)\]
Open your gps_data.txt file in Google Earth and save a jpg image of your gps data.
Upload the files gps_data.txt, angle_data.txt, and your jpg Google Earth image to Canvas.

See file print_pose_data_from_bagfile.py

Hint

Python’s math library has a atan2(y,x) function.

Note

Remember there is a list of common ROS 2 messages. Make sure to read how PoseStamped, Odometry, and NavSatFix work!

Task 3: Write a node named `pose_estimator`

Write a node named pose_estimator that you will run when you play back your bag data.

This node will subscribe to the gps topic and the odometry topic.
- Each time a gps topic is read:
  store the current x and y from the latitude and longitude from the message as a self variables.
  
  More information on how to get the position can be found in the GPSData Basics section.
- Each time an odometry topic is read:
  calculate the orientation angle like in Task 2,
  
  store the angle, speed, z/w orientations as self variables,
  
  and then dead reckon the new x/y positions using same self x,y variables.
  
  More information can be found in the Odometry and Dead Reckoning section.
This node will publish a PoseStamped message, from geometry_msgs, to the vehicle_pose topic. More information can be found in the PoseStamped section.

Upload your python file that defines this node to Canvas.

GPSData Basics

To convert the GPS data from latitude and longitude to UTM coordinates you can use the python library utm. You can install through

pip3 install utm

Then you can call it as follows:

utm.from_latlon(latitude,longitude)

This will give you data in the form (Easting, Northing, Zone Number, Zone Letter). You only need Easting and Northing which Easting is x and Northing is y for the pose.

Odometry and Dead Reckoning

The odometry topic outputs an Odometry message which has information on its structure here. The values for the velocity and orientation are in twist.twist.linear.x, pose.pose.orientation.z and pose.pose.orientation.w. You will use this data to dead reckon. To convert the orientation data from quaternion to a \(\theta_{veh}\) do the following

\[\theta_{veh} = 2*atan2(z , w)\]

theta = 2*math.atan(z,w)

To dead reckon you will estimate the position based on the velocity and angular data, while updating with the GPS Data. For this you have two callbacks one that dead reckons and one that updates the GPS position.

To dead reckon you will do the following,

\[\begin{split}x = x + v \cos (\theta) \Delta t , \\ y = y + v \sin (\theta) \Delta t\end{split}\]

where, \(\Delta t\) is 0.05s. Then every time the GPSData comes in you will update your x with that. store it in a self. variable to pass it between the callbacks. Make sure the x in above is also being stored in a self variable.

A Power point on Dead Reckoning is available here

Important

Both gps and odometry callback functions will update the same self. x and y variables. gps callback will simply store the current x and y based on the GPS’s latitude and longitude. odometry estimates (dead reckons) the future x and y position based on the equations above.

PoseStamped

Make a timer that publishes 20 times a second (20 Hz). In the timer callback, publish a PoseStamped message that takes the current x position, y position, z orientation, and w orientation.

Some information for that is available below:

msgEst = PoseStamped()
msgEst.pose.position.x = # put x here
msgEst.pose.position.y = # put y here
msgEst.pose.orientation.z = # pass in the odometry pose.pose.orientation.z
msgEst.pose.orientation.w =  # pass in the odometry pose.pose.orientation.w
msgEst.header.stamp = self.get_clock().now().to_msg()

Important

PoseStamped has both position and orientation x, y, and z member variables. Make sure to update the correct x, y, and z value when publishing!

Plotting Data (Optional)