Calculation of yaw differences does not generallize

[wohnjayne]

The calculation of yaw differences does not generalize (e.g., to angles > 2*Pi, or if one angle is negative).

The current cost calculation takes the angle difference directly:

stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \
                     self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2

Proposed fix: instead, calculate the generalized yaw diff first:

yaw_norm = np.mod(yaw + 2 * np.pi, 2 * np.pi)
ref_yaw_norm = np.mod(ref_yaw + 2 * np.pi, 2 * np.pi)
diff = float(ref_yaw_norm - yaw_norm)
yaw_diff = abs((diff + np.pi) % (2 * np.pi) - np.pi)
stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \
                     self.stage_cost_weight[2]*(yaw_diff)**2 + self.stage_cost_weight[3]*(v-ref_v)**2

(note: needs to be fixed in multiple places)

[MizuhoAOKI]

@wohnjayne
Thank you for the suggestion!
I'm currently very busy, so I won't be able to address it immediately, but I'll look into it when I have some time. Anyway, I'll assign this task to myself.