Unable to see Rviz movement from odometry
How do you diagnose why Rviz isn't showing any movement from when your robot publishes odometry messages?
I have a simple Python node collecting encoder counts and converting them to odometry messages, which I adapted from this example:
class WheelTrackerNode(object):
def __init__(self):
self._PreviousLeftEncoderCounts = None
self._PreviousRightEncoderCounts = None
self.x = 0.
self.y = 0.
self.th = 0.
self.vx = 0.
self.vy = 0.
self.vth = 0.
self.deltaLeft = 0
self.deltaRight = 0
self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
#self.odom_broadcaster = tf.TransformBroadcaster()
self.odom_broadcaster = tf2_ros.TransformBroadcaster()
self.last_time = None
def update_left(self, count):
"""
Called when the left encoder generates a tick.
"""
if self._PreviousLeftEncoderCounts is not None:
self.deltaLeft = count - self._PreviousLeftEncoderCounts
self.vx = self.deltaLeft * DistancePerCount
self.update()
self._PreviousLeftEncoderCounts = count
def update_right(self, count):
"""
Called when the right encoder generates a tick.
"""
if self._PreviousRightEncoderCounts is not None:
self.deltaRight = count - self._PreviousRightEncoderCounts
self.vy = self.deltaRight * DistancePerCount
self.update()
self._PreviousRightEncoderCounts = count
def update(self):
self.current_time = rospy.Time.now()
if self.last_time is not None:
# compute odometry in a typical way given the velocities of the robot
dt = (self.current_time - self.last_time).to_sec()
delta_x = (self.vx * cos(self.th) - self.vy * sin(self.th)) * dt
delta_y = (self.vx * sin(self.th) + self.vy * cos(self.th)) * dt
delta_th = self.vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
# since all odometry is 6DOF we'll need a quaternion created from yaw
#geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
odom_quat = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, self.th))
# first, we'll publish the transform over tf
#geometry_msgs::TransformStamped odom_trans;
odom_trans = TransformStamped()
odom_trans.header.stamp = self.current_time
odom_trans.header.frame_id = "odom"
odom_trans.child_frame_id = "base_link"
odom_trans.transform.translation.x = self.x
odom_trans.transform.translation.y = self.y
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = odom_quat
# send the transform
self.odom_broadcaster.sendTransform(odom_trans)
# next, we'll publish the odometry message over ROS
#nav_msgs::Odometry odom
odom = Odometry()
odom.header.stamp = self.current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = odom_quat
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = self.vy
odom.twist.twist.angular.z = self.vth
# publish the message
self.odom_pub.publish(odom)
self.last_time = self.current_time
I've confirmed this works by running rostopic echo /odom
when my encoders are turning and seeing messages like:
header:
seq: 600
stamp:
secs: 1481599927
nsecs: 382975101
frame_id: odom
child_frame_id: base_link
pose:
pose:
position:
x: -345.517481299
y: -529.732867745
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ...
Not an answer, but just something to check: translations are always in meters in ROS, so this robot is over 300m away in X, and over half a kilometre in Y. Probably just test data, but perhaps check your scaling?