Publish support leg information to diagnostic
Merge pull request #138 from garaemon/publish-diagnostics

Publish diagnostics from footcoords about support leg