[Graceful Controller] Incorrect Motion Target Heading output by controller

[SakshayMahna]

Bug report

Required Info:

• Operating System:
  • Ubuntu 24.04
• Computer:
  • All Computers
• ROS2 Version:
  • ROS2 Rolling
• Version or commit hash:
  • 719d711
• DDS implementation:
  • All DDS Implementation

Steps to reproduce issue

1. Enable Graceful Controller Plugin
2. Perform any Navigation Workflow

More details here: #5003 (comment)

Expected behavior

The motion target heading for the controller generated path should be in the direction of robot motion.

Actual behavior

The motion target path comes out to be opposite.

Reproduction instructions

Additional information

[SteveMacenski]

Merging shortly