Robotics StackExchange | Archived questions

Problem with amcl (laser will not align with map after moving)

I have created a map using hectorslam, loaded the map via mapserver. Run my odometry and amcl for localization, at first the laserscan aligns with the map but then when i move the robot using teleoptwistkeyboard the laserscan gets mis aligned. Is this suppose to act this way? I tried changing the Fixed Frame from(laser,map,odom,base_link) and all has the same result. Can anyone identify whats causing the problem? Thanks in advance :)

ROS melodic ROS Master: Raspberry pi 4 ROS Slave: Raspberry pi 4

Master terminal:

1.roscore
2.mimi.launch

Slave terminal:

mimi.launch

Ros Master Launch file:

<launch>

    <node pkg="amcl" type="amcl" name="amcl" output="screen">
       <!-- Publish scans from best pose at a max of 10 Hz -->
      <param name="odom_model_type" value="diff"/>
      <param name="odom_alpha5" value="0.1"/>
      <param name="transform_tolerance" value="0.2" />
      <param name="gui_publish_rate" value="10.0"/>
      <param name="laser_max_beams" value="30"/>
      <param name="min_particles" value="500"/>
      <param name="max_particles" value="5000"/>
      <param name="kld_err" value="0.05"/>
      <param name="kld_z" value="0.99"/>
      <param name="odom_alpha1" value="0.2"/>
      <param name="odom_alpha2" value="0.2"/>
      <!-- translation std dev, m -->
      <param name="odom_alpha3" value="0.8"/>
      <param name="odom_alpha4" value="0.2"/>
      <param name="laser_z_hit" value="0.5"/>
      <param name="laser_z_short" value="0.05"/>
      <param name="laser_z_max" value="0.05"/>
      <param name="laser_z_rand" value="0.5"/>
      <param name="laser_sigma_hit" value="0.2"/>
      <param name="laser_lambda_short" value="0.1"/>
      <param name="laser_lambda_short" value="0.1"/>
      <param name="laser_model_type" value="likelihood_field"/>
      <!-- <param name="laser_model_type" value="beam"/> -->
      <param name="laser_likelihood_max_dist" value="2.0"/>
      <param name="update_min_d" value="0.2"/>
      <param name="update_min_a" value="0.5"/>
      <param name="odom_frame_id" value="odom"/>
      <param name="resample_interval" value="1"/>
      <param name="transform_tolerance" value="0.2"/>
      <param name="recovery_alpha_slow" value="0.0"/>
      <param name="recovery_alpha_fast" value="0.0"/>
    </node>
    <!--node pkg="tf" type="static_transform_publisher" name="base_laser" args="0 0 0 0 0 0 base_link laser 100"/-->
    <node pkg="teleop_twist_keyboard" name="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen"/> 
    <node pkg="rviz" type="rviz" name="rviz"/>

</launch>

Ros Slave Launch file:

 <launch>
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
  <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
  <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  </node>
<node pkg="mimi_2dnav" name="odom" type="odom.py" output="screen"/>
<node pkg="mimi_2dnav" name="subscrib" type="subscrib.py" output="screen"/>
<node pkg="map_server" type="map_server" name="map_server" args="/home/pi/catkin_workspace/src/navigation/map_server/m.yaml" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.15 -0.1 0.0 0.0 base_link laser 100" />
</launch>

Odometry publisher:

#!/usr/bin/env python
import math
from math import sin, cos, pi
import RPi.GPIO as GPIO
import rospy
import numpy as np
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
rospy.init_node('odometry_publisher')
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(12, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
right_enc=0
left_enc=0
temp_right_enc=0
temp_left_enc=0
def handle1(self):
    global right_enc
    right_enc=right_enc+1
def handle2(self):
    global left_enc
    left_enc=left_enc+1 
GPIO.add_event_detect(13, GPIO.RISING, handle1)
GPIO.add_event_detect(12, GPIO.RISING, handle2)
x = np.float32(0.0)
y = np.float32(0.0)
th = np.float32(0.0)
dist_bet_wheels=.155
vy = np.float32(-0.1)
current_time = rospy.Time.now()
last_time = rospy.Time.now()
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
    getting encoder ticks
    left_dTick=left_enc-temp_left_enc
    right_dTick=right_enc-temp_right_enc
    temp_left_enc=left_enc
    temp_right_enc=right_enc

    #computing travel distance of each wheel
    dL=0.054*pi*(left_dTick/12.0)
    dR=0.054*pi*(right_dTick/12.0)

    # compute odometry in a typical way given the velocities of the robot
    current_time = rospy.Time.now()
    dt = (current_time - last_time).to_sec()
    last_time = current_time
    vL=dL/dt
    vR=dR/dt
    vth = (vR - vL)/ dist_bet_wheels
    vx= (dR + dL)/ 2.0
    vy=0
    delta_x = ((vx * cos(th)) - (vy * sin(th))) * dt
    delta_y = ((vx * sin(th)) + (vy * cos(th))) * dt
    delta_th = vth * dt

    x += delta_x
    y += delta_y
    th += delta_th


    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x, y, 0.0),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x, y, 0.0), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
    # publish the message
    odom_pub.publish(odom)   
    r.sleep()

Screenshots: rqt_graph: image description

tf view_frames: image description

before moving: image description

after moving: image description

Asked by mimi on 2020-03-02 06:24:04 UTC

Comments

Does moving straight work but rotation is bad?

Asked by stevemacenski on 2020-03-02 12:19:50 UTC

We've been tinkering with the odometry for quite a while now, there were settings that makes moving straight align with the map and the rotation is just bad. But for this settings both rotating and moving straight will destroy the alignment. Do you think the problem is all on the odometry?

Asked by mimi on 2020-03-02 12:24:35 UTC

If you move forward and it works fine, then yes, its probably your odometry. I'd move slowly in rotation and see if you see what's happening (wrong sign, wrong wheel bias distance, etc). If your wheel bias (distance between wheels) is wrong, you're going to have lots of drift problems that will only manifest in rotations because straight forward both motors are spinning at the same speed forward so there's no contribution.

Asked by stevemacenski on 2020-03-02 12:26:22 UTC

The distance between wheels is 155mm, so thats 0.155 m. Should I increase/decrease the value to get a usable odometry?

Asked by mimi on 2020-03-02 18:52:58 UTC

Try it I suppose.

Asked by stevemacenski on 2020-03-02 19:24:08 UTC

Answers

We fixed it. We rotated the laserscanner by 180° and increased the wheel base by about 27mm. Adjusted the amcl tolerance and odom alphas. Thank you so much for the insight.

Asked by mimi on 2020-03-02 22:56:42 UTC

Comments

Yeah, a flipped laser and 3cms would do it! Can you close the question as resolved?

Or @gvdhoorn use your admin magic to move this dialog to an answer to accept ;-)

Asked by stevemacenski on 2020-03-03 01:24:53 UTC

Only a moderator, but there you go.

Asked by gvdhoorn on 2020-03-03 02:25:18 UTC

It took me awhile to realize you could do that. I thought I was going crazy on occasion when my comment appeared as an answer and vise versa ;-)

Asked by stevemacenski on 2020-03-03 10:16:55 UTC

@mimi My Rplidar A1 is also looking backwards due to my design choices, can you tell me if you physically rotated the lidar or just from tf, etc?

Asked by berk tepebag on 2020-04-16 02:15:11 UTC

Physically, although changing the tf values should work too I guess.

Asked by mimi on 2020-04-16 02:21:39 UTC

I have changed it physically but no luck. Is this problem looking similar the one you had?

Asked by berk tepebag on 2020-04-16 07:08:45 UTC

Quite similar. Just to be sure, do you have odometry from wheel encoders?

Asked by mimi on 2020-04-16 07:52:57 UTC

Also, try changing the parameters in amcl and double check your distance between wheels as it has great impact even just a cm difference.

Asked by mimi on 2020-04-16 07:54:21 UTC

Yes I have IMU and wheel encoders combined with EKF. It works good -I believe- since gmapping is working pretty fine even at high speeds (3 m/s). I am just checking each parameter in amcl. Only physically reversing the lidar to original position worked -a bit- as I mentioned before. At least, now it drifts only in forward direction in RVIZ. I will keep trying different wheel_base values. One last question, can you share global_costmat_params and local_costmap_params that worked for you? Thanks for the answers by the way, best wishes :)

Asked by berk tepebag on 2020-04-16 08:14:32 UTC

Base on my experience, though I'm not pretty sure. If it drifts when going forward, the odometry calculation maybe incorrect but since it worked on gmapping that should not be the issue. Did you try to localize using keyboard controls? Do you have same results? Also I cannot send you the params since it was our school project and we left it in school and I can't get out due to the pandemic. Good luck, hope you solve this issue

Asked by mimi on 2020-04-16 08:23:25 UTC

That's the next thing I want to try, if I can command with joystick and localize with amcl then it should also work for teb local planner. Thanks for the help, hope corona will soon be gone and everyone will be fine. Best

Asked by berk tepebag on 2020-04-16 08:37:06 UTC

Please do try and get back with the result. Though I'm not an expert but I will try to help.

Asked by mimi on 2020-04-16 08:39:14 UTC