Leo autonomous navigation tutorial

Software
#1

Hi @Blazej_Sowa,

I’m trying to adapt the Leo Rover Developer Guides - Autonomous Navigation tutorial to my setup.

I was able to adapt the topics to get a similar node graph.

leo_navigation_tutorial_adapted
leo_navigation_tutorial_adapted2475×864 221 KB

The issue I get is that even if the odom frame exists the LeoRover description isn’t shown in RViz. I’m using namespace so modified robot_description to leo/robot_description in the RViz window. The model is displayed when I change the fixed frame from odom to base_footprint.

image
image2099×1303 366 KB

There seem to be an issue with the odom to robot TF but I think my tree is properly setup.

$ rosrun tf tf_echo /odom /realsense_imu_optical_frame
At time 1639426656.367
- Translation: [-42.402, 40123.332, 0.231]
- Rotation: in Quaternion [0.532, -0.511, 0.468, -0.487]
            in RPY (radian) [-1.658, -0.000, -1.530]
            in RPY (degree) [-95.000, -0.000, -87.666]
$ rosrun tf tf_echo /odom /base_footprint             
At time 1639426669.505
- Translation: [-98.301, 41470.026, -0.000]
- Rotation: in Quaternion [-0.000, 0.000, 0.021, 1.000]
            in RPY (radian) [-0.000, 0.000, 0.043]
            in RPY (degree) [-0.000, 0.000, 2.447]

image
image2451×946 390 KB

Do you see anything obvious that I might be missing?

#2

Is the /realsense/imu topic the culprit? Should the orientation (x,y,z,w) values be non zero?

rostopic echo /realsense/imu -n 1
header: 
  seq: 206158
  stamp: 
    secs: 1639426913
    nsecs:  52158594
  frame_id: "realsense_imu_optical_frame"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00523598771542
  y: 0.0
  z: 0.00349065847695
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: -0.12748645246
  y: -9.57129001617
  z: 0.411879301071
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

Here’s the /odometry/filtered output:

rostopic echo /odometry/filtered -n 1
header: 
  seq: 37755
  stamp: 
    secs: 1639427140
    nsecs: 904238939
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: -6364.32784359
      y: 103755.501175
      z: 5.83617535177e-27
    orientation: 
      x: 9.1537298832e-25
      y: -1.47592951365e-24
      z: 0.0767939226193
      w: 0.99704698658
  covariance: [428764852728.86066, 32325976071.85244, 3.1913840655349265e-26, 2.996006058650354e-26, -6.064582541336202e-26, -5186692.6546927765, 32325976071.852715, 2567248797.7187767, -5.121594615469847e-25, 6.0241001779795036e-27, -9.397093556241824e-27, -420225.93061737926, 3.191453223209511e-26, -5.121593543865204e-25, 4.991689687959491e-07, 2.2245545426848255e-12, -2.7729352027131484e-18, -4.198211991401605e-30, 3.135476921941272e-26, 6.240210767443945e-27, 2.224554542684826e-12, 4.983416351776781e-07, 7.422336280383743e-21, -2.4898574872641778e-26, -6.064582542101097e-26, -9.39709355742703e-27, -2.772935202713148e-18, 7.422336280382881e-21, 4.983416411325654e-07, 4.8023883472728163e-26, -5186692.654692823, -420225.9306173789, -4.048091872377733e-30, -2.3794139717714324e-26, 4.802388346782371e-26, 75.55521254083116]
twist: 
  twist: 
    linear: 
      x: -1.98606548724e-48
      y: 162.219836118
      z: 7.57800688388e-24
    angular: 
      x: 8.38601634272e-20
      y: -1.61760839308e-19
      z: 0.00140477887722
  covariance: [0.022257053033048906, -1.2563504552231858e-48, 1.223456571036326e-35, -1.2625008041886102e-37, 1.6565948877342714e-43, 6.4035234384865545e-49, -1.2563504552231855e-48, 31.52616683340289, 4.545388262744821e-26, -9.449622450221385e-35, 1.2646268133179808e-40, 3.2489611790554995e-46, 1.2234565710363263e-35, 4.545388262744821e-26, 4.987541707754389e-07, 1.3266512526455325e-21, -1.6695090369805254e-27, -9.174212044468711e-33, -1.2625008041886098e-37, -9.449622450221385e-35, 1.3266512526455323e-21, 4.950896422862173e-07, 1.8122291235548307e-19, -1.4405279048319616e-19, 1.6565948877342712e-43, 1.2646268133179803e-40, -1.6695090369805257e-27, 1.812229123554831e-19, 4.950896422862175e-07, 2.778685294768137e-19, 6.119464337102886e-49, 3.1048413451485618e-46, -8.76723774678273e-33, -1.3766247227024636e-19, 2.778685294768114e-19, 0.0035786270035043127]
---
Leo Rover gazebo navigation support (IMU and LIDAR)
#3

I finally figured why I couldn’t see the robot description in RViz, I was waiting too long to open RViz and the rover had disappeared already. Check out the video to see the unexpected behaviour I get https://youtu.be/WB4OZYZJSeQ

I’m wondering what could cause this behaviour?

#4

I just figured the issue! There was some wrong parameters in the ekf matrices.

After reading documentation of robot_localization. I ended up using the following matrices.

<rosparam param="twist0_config">[false, false, false,
                                false, false, false,
                                true, true, false,
                                false, false, true,
                                false, false, false]</rosparam>

<rosparam param="imu0_config">[false, false, false,
                               false, false, true,
                               false, false, false,
                               false, false, true,
                               true, false, false]</rosparam>
#5

You don’t have add the rosparam tags. You can just edit these lines in the YAML config file:

From what I see, you added the velocity along the Y axis reported by wheel odometry. This is fine, I guess. the wheel odometry just reports zeros all the time which is somewhat true.

You also added the rotation around Z axis (aka heading) from IMU into consideration. Looking at the message from the realsense imu, it’s clear it does not provide orientation feedback, so adding it to the config should not improve the estimate.

#6

I did use the config file to modify the params. sorry for the confusion.

For some reason, when the Y axis from the wheel odom is set to false, the estimate is shifting rapidly in the Y direction.

I’ll try recalibrating the IMU of the realsense tomorrow and see if I can set the Y axis of the odom to false.

#7

I recalibrated the IMU of the realsense. I haven’t seen any major difference in the reported values.

As you mentionned my IMU doesn’t provide heading. I removed the rotation around Z from the IMU config matrix.

imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              false, false, true,
              true,  false, false]

As per the robot_localization documentation,

it’s best to feed that value to the filter. As a 0 measurement in this case indicates that the robot cannot ever move in that direction, it serves as a perfectly valid measurement:

I’ve set the twist0 vy to true. If I set that value to false the odom estimate starts to drift quickly in the Y+ direction of the base_footprint frame.

twist0_config: [false, false, false,
                false, false, false,
                true, true, false,
                false, false, true,
                false, false, false]

I now have a functional setup.

Thanks for your help! :slight_smile: