Add subscriber to initialpose topic in kinematic sim