Ask Your Question
0

odometry errors, inaccurate measurement

asked 2021-04-07 06:01:50 -0500

apprentice_user gravatar image

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

1 Answer

Sort by » oldest newest most voted
1

answered 2021-04-07 09:04:31 -0500

I don't understand all of the code, but I can point out one thing.

dt = dt.to_sec()

There is information missing here for less than one second. I think you need to use to_nsec() to make up for the information less than 1 second.

edit flag offensive delete link more

Comments

thanks for your answer. I applied your answer, but not much has changed

apprentice_user gravatar image apprentice_user  ( 2021-04-07 12:24:55 -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: 2021-04-07 06:01:50 -0500

Seen: 19 times

Last updated: Apr 07