ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
![]() | 1 | initial version |
I think that you may be confusing your datatypes.
RVIZ takes in a pose with (x,y,z) position and (x,y,z,w) orientation. The orientation is represented by a quaternion. For a quick refresher on quaternions, check out the Wikipedia page.
Probably what is happening is that you are incorrectly publishing your quaternions.
If you have a given set of rotations, in roll/pitch/yaw, you can convert these to a quaternion using the tf::transformations::quaternion_from_euler function. By default it takes the euler angles in sxyz format (roll, pitch, then yaw).
For a bit of example code (in Python):
quat = tf.transformations.quaternion_from_euler(0,0,theta)
### Insert math into Odom msg so it can be published
odom_msg = Odometry()
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
This is for a differential drive robot, where there is no roll and pitch, and the kinematics are calculated shortly before this snippet of code.
So as an example:
>>> tf.transformations.quaternion_from_euler(0,0,0)
array([ 0., 0., 0., 1.])
>>> tf.transformations.quaternion_from_euler(0,0,math.pi/2)
array([ 0. , 0. , 0.70710678, 0.70710678])
>>> tf.transformations.quaternion_from_euler(0,0,2*math.pi)
array([ -0.00000000e+00, 0.00000000e+00, 1.22460635e-16, -1.00000000e+00])
So, just keep in mind that all parts of the quaternion (x,y,z,w) have to be filled out, not just orientation.z.