[ROS2][refact] Rework pose setting

Tong Furequested to merge
fix/FixSetInitialPose into feat/ROS2

This MR follows the changes in MR !419 (merged).

Merge request reports