Making every robot publish their state and position

[Anonymoustank]

Description

Each robot publishes their state and position on /strategy/positon/robotstate/robot{robot_id}

Associated / Resolved Issue

Resolves add state & position feedback to a topic

Steps to Test

Launch simulation and echo topic /strategy/positon/robotstate/robot{robot_id}