ROS 里程计odometry

最近在研究ROS 里的导航包，里面的Gmapping 算法要求有2 个 输入，一个是激光数据，通过/scan topic 输入，另一个是里程计信息。

那么如何获得里程计呢？

里程计包含2 方面的信息，一方面是位姿（位置和转角），另一方面是速度（前进速度和转向速度）。

速度的 获取：前进速度和转向速度

前进速度： 左右轮的平均速度， 这个是通过电机的编码器获取到的。编码器能记录一定时间内的转过的弧度数，根据这个看算出每个轮子的速度。

转向速度：根据左右轮的在给定时间内的弧度差计算得到

参考文章：http://blog.csdn.net/heyijia0327/article/details/47021861

位姿的获取：位置和姿态

位置的获取：根据上面的前进速度推算出位置。

姿态的获取：根据上面的转向速度推算出转角。

那么在真实的机器人身上是如何发布这个里程计信息呢？以ros_arduino_bridge 为例来说明。

适用于：ROS  上位机 + arduino 下位机 + 小车轮子（编码器） 这个硬件架构。

在ros_arduino_bridge\ros_arduino_python\nodes 下面实现了一个'arduino' ROS node ,

这个node 使用 self.myBaseController.poll()  call  到base_controller.py 这个模块。

使用下面的code 来发布里程计（Odometry）

        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()           

            try:
                left_enc, right_enc = self.arduino.get_encoder_counts()
            except:
                self.bad_encoder_count += 1
                rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
                return
                            
            dt = now - self.then
            self.then = now
            dt = dt.to_sec()
            
            # Calculate odometry
            if self.enc_left == None:
                dright = 0
                dleft = 0
            else:
                dright = (right_enc - self.enc_right) / self.ticks_per_meter
                dleft = (left_enc - self.enc_left) / self.ticks_per_meter

            self.enc_right = right_enc
            self.enc_left = left_enc
            
            dxy_ave = (dright + dleft) / 2.0
            dth = (dright - dleft) / self.wheel_track
            vxy = dxy_ave / dt
            vth = dth / dt
                
            if (dxy_ave != 0):
                dx = cos(dth) * dxy_ave
                dy = -sin(dth) * dxy_ave
                self.x += (cos(self.th) * dx - sin(self.th) * dy)
                self.y += (sin(self.th) * dx + cos(self.th) * dy)
    
            if (dth != 0):
                self.th += dth 
    
            quaternion = Quaternion()
            quaternion.x = 0.0 
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)
    
            # Create the odometry transform frame broadcaster.
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0), 
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(),
                self.base_frame,  // 子坐标系
                "odom"            // 父坐标系
                )
    
            odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth

            self.odomPub.publish(odom)
            

这里有一个疑问：为什么要不停的广播 odom->base_frame 之间的转换呢？原因是物体（子坐标系，也就是机器人本身 base_link） 相对于父坐标系odom 的位置是不停的变化的， 所以需要把这个位姿信息定时发送给tf.

那么既然base_link（robot 本身）相对于odom （这个fixed 坐标系）的位姿信息都告诉 tf 了， 为什么还要向/odom 个topic 发送Odemetry 信息呢？

关于Odometry 的获取是否可以有其他的方式呢？