编码器解算ROS里程计数据

编码器解算ROS里程计数据

小狼@http://blog.csdn.net/xiaolangyangyang

def poll(self):
    now = rospy.Time.now()
    if now > self.t_next:
        # Read the encoders
        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)