Do we want to use geometry_msgs::Pose for this version? Should we not use the Twist message here?