Tried testing using odometry for startPose. Might need to convert the
pose from odometry to PoseStamped?