Odom frame is making the robot to lost its position

asked 2020-04-18 11:22:39 -0600

wallybeam gravatar image

updated 2020-04-20 08:25:43 -0600

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:

image description

Here is the TF Tree that I used normally:

image description

The TF Tree with AMCL and Odom:

image description

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.

image description

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

Comments

How do you know it loses it location? What data do you get that makes you think so? You need to tell what is happening before we can help you.

billy gravatar image billy  ( 2020-04-18 14:07:29 -0600 )edit

Thank you for your feedback. I added some details. I am using Rviz to visualize base_link and laser scans. The laser scans move as the base_link moves. However, when I use laser_scan_matcher to locate my robot, laser scans stay stable. Also I am not able to use AMCL too. When I use it my robots motion makes no sense on the Rviz.

wallybeam gravatar image wallybeam  ( 2020-04-18 14:35:53 -0600 )edit

Can you upload your TF tree?

billy gravatar image billy  ( 2020-04-18 22:28:07 -0600 )edit

Of course, I uploaded the TF tree and the graph of encoder feedback vs cmd_vel.

wallybeam gravatar image wallybeam  ( 2020-04-19 03:10:19 -0600 )edit

I'm not seeing a ODOM to BASE_LINK TF. Shouldn't there be one in there?

billy gravatar image billy  ( 2020-04-19 13:53:53 -0600 )edit

Normally, there is an odom to base_link TF. After I observed a shift in the position of the robot, I removed the amcl node and isolated my odom to see if the problem related to it.

wallybeam gravatar image wallybeam  ( 2020-04-19 14:13:09 -0600 )edit

AMCL will publish the MAP --> ODOM TF. Whatever node publishes ODOM is responsible for ODOM --> BASE_LINK TF. I'm not saying that is the current issue. I see you've posted the python code that publishes the TF. But always easiest to make things are set up properly first.

Can you show the TF tree when using move_base and AMCL?

Can you also verify what Fixed Frame you've set in RVIZ?

For the timing of CMD and encoder, what are units for time? mS? uS?

billy gravatar image billy  ( 2020-04-19 14:44:05 -0600 )edit

I will be able to upload the TF Tree that you requested tomorrow. For now, I uploaded the TF Tree that I got the best performance by using laser_scan_matcher. Actually it is a discrete graph, I sampled the data with a frequency of 30 HZ. I renewed the graph and the delay is around 0.3 seconds.

wallybeam gravatar image wallybeam  ( 2020-04-19 15:54:54 -0600 )edit