Skip to content

Fix the heading published by the simple planning simulator

Maxime CLEMENT requested to merge (removed):fix-simple-planning-simulator-published-yaw into master

This MR fixes a bug in the heading of VehicleKinematicState messages published by the simple_planning_simulator.

Internally, the simulator calculates a yaw that is not bounded by [-pi, pi], leading to a wrong heading calculated by the from_angle common function. This MR fixes this simply by wrapping the angle before converting it.

To reproduce the issue, run the stack with the simple_planning_simulator and just set an initial pose while running ros2 topic echo /vehicle/vehicle_kinematic_state.

  • Without this MR, the imaginary part of the heading is 0, even when changing the initial pose (if you set a trajectory this might change but the heading will still be incorrect).
  • With this MR, the heading should now be correct.

Merge request reports