quaternions with python
I have recently started using rospy after being working with roscpp. I wanted to reproduce this piece of C++ code:
geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(th);
geometry_msgs::PoseStamped goal;
goal.pose.orientation = quat;
For what I have found, the closest form would be something like:
quat = quaternion_to_msg(tf.transformations.quaternion_from_euler(0, 0, th))
goal = PoseStamped()
goal.pose.orientation = quat
But this actually does not work because 'quaternion_from_euler' returns a four float array, instead of an object with {x, y, z, w} fields.
I have searched for some common function to do this easy transform, but I have not found it within the ros libraries. What I have seen is that many people make his own implementation:
def quaternion_to_msg(q):
msg = Quaternion()
msg.x = q[0]
msg.y = q[1]
msg.z = q[2]
msg.w = q[3]
return msg
goal.pose.orientation = quaternion_to_msg(quat)
Does ROS provide a common solution for this situation?