Using teb_local_planner with robot_localization

Question

Hello,

I am trying to get the teb planner (for ackermann vehicle) working with robot_localization ( wheel, IMU, & GPS).

Previously I was using teb planner with amcl for localization with good results. When I switched to using robot_localization (dual ekf navsat https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/params/dual_ekf_navsat_example.yaml) the teb planner began behaving erratically.

The odometry/filtered and odometry/filtered_map from the ekf nodes look good in rviz and don't jump around alot.

However the teb planner now is sending my robot in reverse for long distances until it crashes into a wall. Otherwise it sometimes just sends it in a circle.

My main question is, are there settings that need to be changed to use teb planner with the robot_localization package? what should I use for "odom_topic" in the teb planner? Which "global_frame" should I use for my local and global costmaps? If cmd_angle_instead_rotvel is set to true, will that conflict with twist messages coming back from the ekf_node odometry?

Any help would be greatly appreciated. For reference, my (relevant?) teb local planner parameters are as follows:

TebLocalPlannerROS:

odom_topic: odometry/filtered

teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
global_plan_overwrite_orientation: True
max_global_plan_lookahead_dist: 6.0
feasibility_check_no_poses: 5
max_vel_x: 0.1
max_vel_x_backwards: 0.1
max_vel_theta: 0.5
acc_lim_x: 0.1
acc_lim_theta: 0.5

min_turning_radius: 4.0 
wheelbase: 1.8542                 # Wheelbase of our robot
cmd_angle_instead_rotvel: True 

---

Update: 1

@ufr3c_tjc thanks for the quick response. I have done as you say and changed my global_frame to odom for both the local and global costmap. It seems you have a very similar setup to mine (dual ekf with one continuous and one discrete gps).

However this change does not seem to fix my problem. The interesting thing I have found is that the teb_planner works fine with the robot_localization as long as I don't set the odom_topic to be /odometry/filtered (ie the output of my continuous ekf). If I leave odom_topic as the default odom (which doesnt exist) the teb_planner works fine. Therefore I believe that something in the teb_planner code is conflicting with the odom information being published by the ekf. Could it possibly be that the teb_planner doesn't expect the velocities to be in the base_footprint frame (ie only instantaneous x velocities?)

Any further ideas? Perhaps @croesmann has an idea?

---

Originally posted by biglotusturtle on ROS Answers with karma: 165 on 2017-07-05

Post score: 0

---

Original comments

Comment by ufr3c_tjc on 2017-07-06:
Yeah that is a weird one. Is the continuous ekf node publishing the odom to base_link transform? I'd guess that the planner uses the transform for position, and maybe only the velocity is pulled from the odom message. If you look at DWA local planner it doesn't require an odom topic.

Comment by biglotusturtle on 2017-07-07:
Do you know if the DWA local planner supports ackermann steering?

Answer 1

I'll let you know the details of our setup for working with teb_local_planner and robot_localization:

1. We set up two instances of robot_localization. Both use inputs from wheel odometry and an IMU. One of these is the continuous ekf estimate, which has its world_frame set to odom. The other is the absolute ekf estimate, which has the world_frame set to map. The absolute ekf estimate also has GPS integrated into it, which comes from the navsat_transform_node in the robot_localization package. The continuous ekf publishes the odom -> base_link transform, and the absolute ekf publishes the map -> odom transform.
2. We set up the navigation stack, using appropriate parameters for our robot. The important parameters here are the global_frame for the costmaps. The local costmap runs in odom always. The global costmap can either be odom or map. Which to choose depends on a couple things:
   • The accuracy of your gps. If your GPS has a typical few-metre accuracy, and tends to jump around, your transform from map to odom will also jump around. This will make your goal move, as the goal is set in the global costmap, which uses the map frame. If you have a differential GPS or RTK-GPS, which has an accuracy of a few centimeters, small jumps in GPS data should be handled much better by the ekf node, and your map to odom transform will not jump around as much.
   • The tolerance which you are trying to achieve your goal within. If you are trying to drive to within 10cm of your goal, but your GPS is jumping around by a few metres, the odom to map transform will jump around much more than the 10cm tolerance, and your robot will keep trying to drive to the new position after each transform update (not good). However, if you are trying to drive to within 30cm of the goal, with an accurate RTK-GPS, the jumping in goal position will likely be much less than the tolerance. This means the robot should be able to happily achieve the goal.
3. (really still 2) If you have an accurate GPS and believe it will not cause large jumps in the map -> odom transform, choose map as the global costmap frame id. This has the benefit that your robot will always achieve the world-referenced navigation target (since map is truly world-fixed). If your GPS isn't great, choose odom as the frame for the global costmap. This means the goal will now be odom-frame-referenced. Odom is considered world-fixed, but it isn't really if you're using it for navigation with data sources that are non-absolute (like encoders and an IMU). The odom frame's position in the world will drift over time. This means the robot will likely end up in a spot that isn't quite the goal that you sent it towards. BUT it will still be able to achieve the goal, which it likely wouldn't if you ran with a 'bad' GPS and the map frame. Our GPS isn't great, so we currently use odom as the frame for global costmap, but we have some smart overhead to handle object repositioning and goal transforms. You'll have to do the same if you choose odom, as world-fixed objects will need reconciliation into the map frame (such that the objects become truly world-fixed).
4. Set the following parameters for teb_local_planner:
   • The odom topic is the output from the continuous ekf estimate.
   • The map frame is whichever frame you use for the global costmap.
   • Choose whatever other parameters you need for ackermann, as we use diff drive.

I'd also perform the following checks:

• Joystick the robot around, and check that the ekf estimating nodes are working correctly, to eliminate them as the problem.
• I'd also be sure to check that your IMU is positioned correctly, according to the docs for robot_localization.
• Perform the same sanity check for the GPS, making sure that its data is being transformed correctly into the map/utm frame.

---

Originally posted by ufr3c_tjc with karma: 885 on 2017-07-05

This answer was ACCEPTED on the original site

Post score: 6