There are three major launch files to use:

• path_record.launch
  • This is used to record the vehicle state in a demonstrated trajectory.
  • The launch file asks for a matfile of waypoints: this is just to aid in the visualization and isn't used for control.
• sim_path_follow.launch
  • This uses a dynamic bicycle vehicle model to simulate following the demonstrated trajectory (matfile of waypoints).
  • Initial vehicle pose is required.
• path_follow.launch
  • This is used to publish commands to the actual vehicle.
• Common settings:
  • Track path using time (i.e. varying velocity) or with a fixed speed.
  • Reference system is defined based on (lat0, lon0) as the global origin.
  • Use heading (cw from North) or psi (ccw from East). For the Genesis, we can use the latter one.

The launch files use the following scripts:

• mpc_cmd_pub.jl

  • Subscribes to the state from the vehicle (or vehicle simulator).
  • Queries a reference generator (ref_gps_traj.py) to get the desired open-loop trajectory. A full global trajectory is provided ahead of time in mat format from the launch file.
  • Uses a nonlinear kinematic MPC model with IPOPT to generate the optimal acceleration and steering angle (tire) commands
  • Publishes the commands to the vehicle's low level controller, along with the desired open-loop trajectory (target_path) and the predicted open-loop trajectory MPC solution (mpc_path).

• vehicle_simulator.py

  • This is for sake of tuning algorithms. It uses a dynamic bicycle model for high model fidelity.
  • Low-level P controller used to simulate the fact that acceleration/steering angle control inputs are tracked with some delay. This P gains can be tuned to simulate timing delays on the real vehicle.
  • The topics used for control match the Genesis Interface exactly. Thus this module can be swapped with the real vehicle interface with no issues.

• plot_gps_traj.py

  • This is a simple visualizer of the path following behavior.
  • The global trajectory is plotted in black and kept fixed.
  • The vehicle's current state (blue), target path (open-loop reference, in red), and predicted MPC path (green) are updated via subscriber callbacks.

• state_publisher.py

  • Used for the real vehicle, in place of vehicle_simulator.py.
  • Subscribes to GPS/IMU topics and provides a synchronized version of the vehicle state for convenience.

Mat Format for Reference Path (used by ref_gps_traj.py):

• Keys: 't', 'x', 'y', 'v', 'psi', 'a', 'df', 'lat', 'lon' (i.e. the state information and ROS time)
• Values: each key is associated with a N-long array, where N is the number of state samples recorded
• There is also a 'mode' entry (Real, Sim, or Follow) to aid with plotting/analysis and keeping track of data.

Contact Vijay Govindarajan ([email protected]) with any questions or comments.