Fix setting the quaternion.
Fix setting the quaternion.

m312z/ROSPlan,m312z/ROSPlan,m312z/ROSPlan,m312z/ROSPlan,KCL-Planning/ROSPlan,m312z/ROSPlan,KCL-Planning/ROSPlan,KCL-Planning/ROSPlan,KCL-Planning/ROSPlan