initialize_origin and origin_manager: add heading support

[DangitBen]

Neither initialize_origin.py nor origin_manager support a heading offset.

At least initialize_origin.py should be able to support this (maybe both) as it feeds the information to a swri_transform_util object with a LocalXyUtil that can support a heading offset.

I believe the message being published also supports and orientation, which should be populated with this information.