In this HW, you will implement PID controller to follow waypoints along a path on the simulator.

At this link, you will find a code template, as below:

• gem_waypoint_pid
• waypoints
• wps.csv a list of waypoints
• records_wps a visualization of the waypoints
• scripts
• gen_waypoint.py for generating waypoints
• pid.py for implementing the PID controller
• follow_waypoints.py main script to interact with the simulator

Fill in the code snippets marked with "# TODO: ..." with something that makes sense.

• PID Controller Please implement PID controller in `gem_waypoint_pid/scripts/pid.py`.
• Application of PID controller Please complete the following parts in `gem_waypoint_pid/scripts/follow_waypoints.py`:
  • transforming the goal point into the vehicle coordinate frame in function `start_drive`
  • define your feedback value for PID control in function `start_drive`
  • Set your own sets of weights for P, I, D terms in `__init__`.

How to run

Assume your ROS workspace locates at `$WS_ROOT`. Please place folder `gem_waypoint_pid` under `$WS_ROOT/src/POLARIS_GEM_e2/polaris_gem_drivers_sim`.

Then `rosrun gem_waypoint_pid follow_waypoints.py`. You should see vehicle move in the middle of the road in the simulator.

What to be submitted

Please submit the following:

1. Your completed `gem_waypoint_pid`
2. Two videos showing the running of your PID contollers with two sets of P, I, D terms.