Added possibility to publish geometry_msgs/PoseArray with the simulation
and localization pose.