Odom frame is making the robot to lost its position

edit

I am using the map frame as a fixed frame in Rviz I am also using DWA Local Planner and Base Local Planner. My robot has 4 wheels with 4 motors and I am using it as a differential drive robot.

I am using the following code to generate the Odom frame from encoder readings of my robot. In order to test whether it works, I drive it forward, backward and I make it rotate in-place a few times. In this test, everything worked well. However, when the robot makes rotations with a rotation radius different than zero, it loses its location.

Here are my observations:

• I make the frame id map and the child frame id base_link. The following code publishes the relation between them. After that, I used Rviz to visualize the laser_frame. The scanned objects move as the vehicle moves.
• I used AMCL and observe the base_link then I used move_base to make the vehicle move. The vehicle almost teleports out of the map, so I think there is something wrong. map>odom>base_link is the relation between the frames.
• Lastly, I used laser_scan_matcher to relate the map with base_link. This configuration works better, but it is not good enough. In some hallways, the robot is not moving but in reality, it moves.

Here is the TF Tree that I used for testing the odom:

Here is the TF Tree that I used normally:

The TF Tree with AMCL and Odom:

I also investigated the encoder data it seems fine. I couldn't find out the problem. I have a four-wheeled robot with four motors yet I am using differential drive algorithms. Can it be the problem?

Here is a time vs speed graph. Red lines represent my encoder feedback and the blue line is the speed command from cmd_vel. As you can see graphs are almost identical with a delay. The units of the x-axis is sample and mm/s for the y-axis. I took 30 samples per second so 3.3 second passes for 100 samples. I calculated a delay of 0.3 seconds.

#!/usr/bin/python
# This beautiful state of art publishes tf between odom and base_link
import rospy
import roslib
import math 
import numpy
import tf2_ros
import tf_conversions


# Messages
from std_msgs.msg import Float32
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Quaternion, Twist, TransformStamped

# Use left and right angular velocities to compute robot unicycle velocities
# Publish the estimated velocity update
class OdomPublisher:
  def __init__(self):
    rospy.init_node('diffdrive_odom')
    self.lwheel_angular_vel_enc_sub = rospy.Subscriber('lwheel_angular_vel_enc', Float32, self.lwheel_angular_vel_enc_callback)    
    self.rwheel_angular_vel_enc_sub = rospy.Subscriber('rwheel_angular_vel_enc', Float32, self.rwheel_angular_vel_enc_callback)    
    self.lwheel_tangent_vel_enc_pub = rospy.Publisher('lwheel_tangent_vel_enc', Float32, queue_size=10)
    self.rwheel_tangent_vel_enc_pub = rospy.Publisher('rwheel_tangent_vel_enc', Float32, queue_size=10)
    self.cmd_vel_enc_pub = rospy.Publisher('cmd_vel_enc', Twist, queue_size=10)

    self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
    self.equalizer = 0.39
    self.L = rospy.get_param('~robot_wheel_separation_distance', 0.86) 
    self.R = rospy.get_param('~robot_wheel_radius', 0.075)
    self.rate = rospy.get_param('~rate', 10)
    self.N = rospy.get_param('~robot_wheel_ticks', 20)
    self.frame_id = rospy.get_param('~frame_id','/odom ...