rotatePose

public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading)

This rotates the given pose by the given theta,

Return

the rotated Pose.

Parameters

pose

the Pose to rotate.

theta

the angle to rotate by.

rotateHeading

whether to adjust the Pose heading too.