Ask Your Question
0

Unable to see Rviz movement from odometry

asked 2016-12-21 08:49:21 -0500

Cerin gravatar image

updated 2016-12-24 23:15:28 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

position: 
  x: -345.517481299
  y: -529.732867745

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?

gvdhoorn gravatar imagegvdhoorn ( 2016-12-21 09:23:14 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-12-22 03:13:38 -0500

Akif gravatar image

Try odom as your fixed frame in rviz. It seems like it is set as base_link.

edit flag offensive delete link more

Comments

This was the issue. After I switched it, I couldn't tell at first, since the line from odom to base_link wasn't showing due to an incorrect scale, but after I fixed the scaling, the base_link was in view, clearly showing the link change as odometry updated.

Cerin gravatar imageCerin ( 2016-12-24 23:14:57 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-12-21 08:49:21 -0500

Seen: 330 times

Last updated: Dec 24 '16