Hi,
my motion planner always fails when facing some situations that need to turn around. Can you give some suggestions for solving this problem？
I write some code to calculate orientation, maybe there are some thing wrong. I want to make it turn to the right direction following the lane.
Thanks a lot.
#orientation_difference
#calc_angle_to_goal function:
#Returns the orientation of the goal (angle in radian, counter-clockwise defined)
#with respect to the position of the state.
angleToGoal = self.calc_angle_to_goal(path[-1])
orientation_difference = abs(orientation_diff(angleToGoal, path[-1].orientation))
#Returns lanelet orientation (angle in radian, counter-clockwise defined)
#at the given position and lanelet id.
laneletOrientationAtPosition = self.calc_lanelet_orientation(final_lanelet_id[0], path[-1].position)
orientation_difference_lanelet = abs(orientation_diff(self.desired_orientation,laneletOrientationAtPosition))
