odometry errors, inaccurate measurement
I have wheel encoders, IMU sensor (MPU 9250 9 dof), rplidar a2. I cannot proceed because ı made a mistake while doing odometry. I'm mapping with Lidar, I'm using Imu sensor with AHRS. I know the next step is localization. I noticed that odometry is not working properly for localization. I saw an error occurred because it was very fast data transfer with between STM32 - jetson nano and I corrected it. So more specific problem is my calculations are getting wrong on when encoder data finishes one turn and the data start from 0 and increase till max number (it increase if it is going forward). So I wanted to get rid of this problem but I stuck at that point. Rest of the process is fine for now but this threshold of encoder data makes driver useless. where am I doing wrong. How can I solve this problem. Or can you suggest an alternative solution.
#!/usr/bin/env python
import roslib
import rospy
import os
import numpy as np
from math import pi, sin, cos
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, Quaternion, Pose
from tf.broadcaster import TransformBroadcaster
from rospy.client import init_node
import geometry_msgs
from geometry_msgs.msg._TransformStamped import TransformStamped
from std_msgs.msg._Int32MultiArray import Int32MultiArray
import std_srvs.srv
class BaseController():
def __init__(self):
rospy.init_node("Base_control")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
self.rate = 10
self.base_frame_id = rospy.get_param('~base_frame_id', 'chassis')
self.base_frame_id = rospy.get_param('~base_frame_id', 'footprint')
self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom')
self.encoder_right = 0.
self.encoder_left = 0.
self.base = 0.18
self.wheelR = 0.1
self.wheel_ticks = 940.
self.last_encoder_left = 0.
self.last_encoder_right = 0.
self.x = 0.0
self.y = 0.0
self.th = 0.0
self.last_time = rospy.Time.now()
self.ticks_per_meter = 1253.33
self.delta_left = 0.0
self.delta_right = 0.0
self.delta_xy = 0.0
self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odom_broadcaster = TransformBroadcaster()
rospy.Subscriber('encoders', Int32MultiArray, self.encoders_cb )
self.init_left = self.encoder_left
self.init_right = self.encoder_right
self.reset_service = rospy.Service('odom_reset', std_srvs.srv.Empty, self.odom_reset_cb)
self.getinit = False
def encoders_cb(self, msg):
self.encoder_right = msg.data[1]
self.encoder_left = msg.data[0]
def spin(self):
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
self.update()
r.sleep()
def update(self):
if self.getinit == False and self.encoder_right!=0 and self.encoder_left!=0:
print("okudum")
self.getinit=True
self.delta_left = 0
self.delta_right = 0
self.last_encoder_left =self.encoder_left
self.last_encoder_right = self.encoder_right
timestamp = rospy.Time.now()
dt = timestamp - self.last_time
dt = dt.to_sec()
if self.delta_left != 0:
tempL = (self.encoder_left - self.last_encoder_left) / self.ticks_per_meter
if abs(tempL / (self.delta_left)) > 3.5: # encoder data finishes one turn solutions
c = 1
if tempL > 0:
c=-1
self.delta_left = (self.encoder_left + c * self.wheel_ticks - self.last_encoder_left) / self.ticks_per_meter
else :
self.delta_left = (self.encoder_left - self.last_encoder_left) / self.ticks_per_meter
else:
self.delta_left = (self.encoder_left - self.last_encoder_left) / self.ticks_per_meter
if self.delta_right != 0:
tempR = (self.encoder_right - self.last_encoder_right) / self.ticks_per_meter
if ...