# TEB Tuning Guide ## How to refine `teb_local_planner` for navigation This guide is for you if you choose to use `teb_local_planner` to navigate your robots. For more information, [click here](http://wiki.ros.org/teb_local_planner). ### 1. Setting up #### 1.1. Configure local planner > Filepath: catkin\_ws/movel\_ai/config/movel/config/ > > File to modify: **base\_local\_planner\_params.yaml** Note that config files for local planner is located in the `movel` package. In the yaml file itself, you see there are actually 3 local planners included. But we will only use one of them. Uncomment the entire long section under `TebLocalPlannerROS`. {% hint style="info" %} One good practice will be to include only things you need (uncomment if previously commented) and comment out the rest. {% endhint %} These are the parameters that can be configured. Will explain how to tune these parameters later. {% tabs %} {% tab title="base\_local\_planner\_params.yaml" %} ```yaml TebLocalPlannerROS: odom_topic: /odom #odom_topic: /rtabmap/odom map_frame: map # Trajectory teb_autosize: True dt_ref: 0.3 #0.2 dt_hysteresis: 0.1 global_plan_overwrite_orientation: True max_global_plan_lookahead_dist: 3.0 feasibility_check_no_poses: 5 # Robot max_vel_x: 0.40 #0.25 max_vel_x_backwards: 0.4 #0.2 max_vel_theta: 1.0 #0.5 acc_lim_x: 0.5 # 0.2 acc_lim_theta: 0.6283 #0.5, 0.26 min_turning_radius: 0.0 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "polygon" #"circular" radius: 0.38 # for type "circular" #line_start: [-0.3, 0.0] # for type "line" #line_end: [0.3, 0.0] # for type "line" #front_offset: 0.2 # for type "two_circles" #front_radius: 0.2 # for type "two_circles" #rear_offset: 0.2 # for type "two_circles" #rear_radius: 0.2 # for type "two_circles" vertices: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ] # for type "polygon" # GoalTolerance xy_goal_tolerance: 0.2 #0.2 yaw_goal_tolerance: 0.1571 free_goal_vel: False # Obstacles min_obstacle_dist: 0.05 inflation_dist: 0.0 dynamic_obstacle_inflation_dist: 0.05 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 0.1 obstacle_poses_affected: 25 #30 costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5 # Optimization no_inner_iterations: 5 #5 no_outer_iterations: 4 #4 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.1 weight_max_vel_x: 2 #2 weight_max_vel_theta: 1 #1 weight_acc_lim_x: 1 # 1 weight_acc_lim_theta: 1 # 1 weight_kinematics_nh: 1000 #1000 weight_kinematics_forward_drive: 1000 #1000 weight_kinematics_turning_radius: 1 #1 #only for car-like robots weight_optimaltime: 1.0 #1 weight_obstacle: 50 #50 weight_viapoint: 5.0 #5.0 #1.0 weight_inflation: 0.1 #0.1 weight_dynamic_obstacle: 10 # not in use yet selection_alternative_time_cost: False # not in use yet # Homotopy Class Planner enable_homotopy_class_planning: False #True enable_multithreading: True simple_exploration: False max_number_classes: 2 #4 roadmap_graph_no_samples: 15 roadmap_graph_area_width: 5 h_signature_prescaler: 0.5 h_signature_threshold: 0.1 obstacle_keypoint_offset: 0.1 obstacle_heading_threshold: 0.45 visualize_hc_graph: False #ViaPoints global_plan_viapoint_sep: 0.5 #negative if none via_points_ordered: False #adhere to order of via points #Feedback publish_feedback: true #false ``` {% endtab %} {% endtabs %} {% hint style="warning" %} **Warn:** yaml forbids tabs. Do not use tabs to indent when editing yaml files. Also, check your indentations align properly. {% endhint %} #### 1.2. Configure velocity setter After configuring Seirios to use teb planner, you need to sync the velocity with maximum velocity specified by teb planner. > Filepath: catkin\_ws/movel\_ai/config/velocity\_setter/config/ > > File to modify: **velocity\_setter.yaml** {% tabs %} {% tab title="velocity\_setter.yaml" %} ```yaml local_planner: "TebLocalPlannerROS" parameter_name_linear: "max_vel_x" parameter_name_angular: "max_vel_theta" ``` {% endtab %} {% endtabs %} Check `local_planner` match the name of the planner in the base local planner configuration. You are configuring the maximum velocities to be the maximum velocities you specified in teb planner. #### 1.3. Checks * [ ] `rostopic list` in terminal to check you have `/odom` published. * [ ] Planners require a map to work so make sure you have already mapped the location you want to navigate in. * [ ] Boot up rviz, change your Fixed Frame to `/map`. * [ ] In Seirios, load the map from the library. ### 2. Localizing your robot **rviz:** Use the 2D Pose Estimator tool to pinpoint the location of your robot on the map. You can use LaserScan to help you – increase the Size (m) to at least 0.05 and try to match the lines from the LaserScan as closely as possible with the map. **Seirios:** Go to Localize. Use either your keyboard or the joystick button to align the laser with the map as closely as possible. #### 2.1. AMCL configurations > Filepath: catkin\_ws/movel\_ai/config/movel/config/amcl.yaml > > File to modify: **amcl.yaml** Amcl configuration file is in the same directory as base\_local\_planner\_params.yaml. Similarly, there are a lot of paramters in the amcl file although we are only interested in these 2 parameters: {% tabs %} {% tab title="amcl.yaml" %} ```yaml min_particles: 50 max_particles: 1000 ``` {% endtab %} {% endtabs %} Configure `min_particles` and `max_particles` to adjust the precision. {% hint style="info" %} You can increase the values if you want a more precise localization. However, tune the values with respects to the size of your map. Too many particles on a small map means there are redundant particles which only wastes computing power. [Click here](http://wiki.ros.org/amcl) for more information. {% endhint %} #### 2.2. Checks * [ ] Robot's position in rviz/Seirios must correspond to the actual position of the robot as accurately as possible. Inaccurate localization could cause problems such as bad routing, robot going to restricted areas, robot getting stuck at awkward corners etc. ### 3. Tuning `teb_local_planner` parameters This section provides some suggestions on what values to give to the long list of parameters for TebLocalPlannerROS. Go back to the same base\_local\_planner\_params.yaml file. {% hint style="info" %} TEB requires a lot of tuning to get it to behave the way you want. Probably a lot of it is through trial and error. For all kinds of robot tuning, always refer to the robot manufacturer’s configs. Whatever params you set must be within what that is handleable by your robot. **General tip:** Make changes to only one/a few parameters at a time to observe how the robot behaves. {% endhint %} Tuneable parameters can be grouped into the following categories * Robot * Goal Tolerance * Trajectory * Obstacles * Optimisation #### 3.1. Robot Tune robot related params with respects to the configurations specified by the manufacturer. i.e. The values set for velocity and acceleration should not exceed the robot's hardware limitations. **Kinematics** `vel` parameters limits how fast the robot can move. `acc` parameters limits how fast robot can accelerate. `_x` specifies linear kinematics whereas `_theta` specifies angular kinematics. ```yaml max_vel_x: 0.40 #0.25 max_vel_x_backwards: 0.4 #0.2 max_vel_theta: 1.0 #0.5 acc_lim_x: 0.5 # 0.2 acc_lim_theta: 0.6283 #0.5, 0.26 ``` **Footprint Model** Literally a footprint. :footprints: Ideally, configure the footprint to be slightly bigger than the actual measurement of the robot. `footprint_model` should be configured with respects to the robot's measurements. ```yaml footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "polygon" #"circular" radius: 0.38 # for type "circular" #line_start: [-0.3, 0.0] # for type "line" #line_end: [0.3, 0.0] # for type "line" #front_offset: 0.2 # for type "two_circles" #front_radius: 0.2 # for type "two_circles" #rear_offset: 0.2 # for type "two_circles" #rear_radius: 0.2 # for type "two_circles" vertices: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ] # for type "polygon" ``` You must indicate the `type` of the robot footprint. Different types include `polygon`, `circular`, `two_circles`, `line`, `point` etc. For simplicity sake, footprints are usually circular or polygon. [Click here](http://wiki.ros.org/teb_local_planner/Tutorials/Obstacle%20Avoidance%20and%20Robot%20Footprint%20Model) for more footprint information. `radius` is required for `type: "circular"` `vertices` is required for `type: "polygon"` #### 3.2. Goal Tolerance Specifies how much deviation from the goal point that you are willing to tolerate. ```yaml xy_goal_tolerance: 0.2 #0.2 yaw_goal_tolerance: 0.1571 ``` `xy_goal_tolerance` is the acceptable linear distance away from the goal, in meters. {% hint style="info" %} Should not set `xy` value too high, else the robot will stop at a very different location. However, some leeway is required to account for robot drift etc. {% endhint %} `yaw_goal_tolerance` is the deviation in the robot's orientation. i.e. Goal specifies that the robot should face directly in front of the wall but in actual, the robot faces slightly to the left. {% hint style="info" %} Should not give a too tight `yaw` tolerance, else the robot could be jerking around just to get the orientation right. Which might not be ideal in terms of efficiency. {% endhint %} #### 3.3. Obstacles Decides how the robot should behave in front of obstacles. Experimentation is required to tune the planner to approach obstacles optimally. A riskier configuration will allow the robot to move in obstacle ridden paths i.e. narrow corridors but it might get itself stuck around obstacles or bump into obstacles. A more conservative configuration might cause the robot to rule out its only available path because it thinks its too close to obstacles. The tricky part is really to achieve a balance between those two scenarios. ```yaml min_obstacle_dist: 0.05 inflation_dist: 0.0 dynamic_obstacle_inflation_dist: 0.05 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 0.1 obstacle_poses_affected: 25 #30 costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5 ``` `min_obstacle_dist` is the minimum distance you want to maintain away from obstacles. `inflation_dist` is the buffer zone to add around obstacles. [Click here](http://wiki.ros.org/teb_local_planner/Tutorials/Obstacle%20Avoidance%20and%20Robot%20Footprint%20Model) for more information about obstacle avoidance and penalty. And [click here](http://wiki.ros.org/teb_local_planner/Tutorials/Frequently%20Asked%20Questions) for known problems with regards to obstacle avoidance tuning. ### 4. Tuning costmap for obstacle avoidance Besides configuring obstacle handling behaviours in local planner, we can also configure the costmaps. A costmap tells a robot how much does it cost to move the robot to a particular point. The higher the cost, the more the robot shouldn’t go there. Lethal obstacles that could damage the robot will have super high cost. > Filepath: catkin\_ws/movel\_ai/config/movel/config/ > > File to modify: **costmap\_common\_params.yaml** File is in the same directory as `base_local_planner_params`. {% tabs %} {% tab title="costmap\_common\_params.yaml" %} ```yaml footprint: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ] # robot_radius: 0.38 #0.38 # footprint_padding: 0.05 map_type: voxel #track_unknown_space: true obstacle_layer: origin_z: -0.1 z_resolution: 1.8 #1.5 This must be higher than the z coordinate of the mounted lidar z_voxels: 1 obstacle_range: 10.0 #10.0 raytrace_range: 15.0 #15.0 observation_sources: laser_scan_sensor track_unknown_space: true lethal_cost_threshold: 100 unknown_cost_value: 255 laser_scan_sensor: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, min_obstacle_height: 0.00, max_obstacle_height: 3.00} #point_cloud_sensor: {sensor_frame: lslidar_c16_frame, data_type: PointCloud2, topic: /lslidar_c16/lslidar_point_cloud, marking: true, clearing: true} lowbstacle_layer: origin_z: -0.1 z_resolution: 1.8 z_voxels: 1 obstacle_range: 3.5 #if beyond this threshold, then will not mark as obstacle raytrace_range: 5.0 #5.0 Lower this value to detect nearer obstacles with better accuracy observation_sources: obs_cloud mock_scan #butt_scan1 butt_scan2 publish_voxel_map: true track_unknown_space: true lethal_cost_threshold: 100 unknown_cost_value: 255 obs_cloud: data_type: PointCloud2 topic: /obstacles_cloud marking: true clearing: true min_obstacle_height: 0.01 max_obstacle_height: 0.99 mock_scan: data_type: LaserScan topic: /obstacles_scan marking: false clearing: true min_obstacle_height: 0.00 max_obstacle_height: 1.00 inf_is_valid: true inflation_layer: enabled: true cost_scaling_factor: 6.0 #added in by John inflation_radius: 0.39 #0.45 #Minimum value: 0.379 dynamic_obstacle_layer: enabled: false map_tolerance: 0.2 footprint_radius: 0.5 range: 2.0 ``` {% endtab %} {% endtabs %} There are several layers added into the costmap. Usually we follow the specifications [here](http://wiki.ros.org/costmap_2d/layered#Parameters). `footprint`must match the measurements specified in base\_local\_planner\_params.yaml. You can choose to add some or all of the layers in the `common_costmap_params` into `global_costmap_params` and `local_costmap_params`. For the costmap layers that you have decided to use, you must mount the layers into `global_costmap_params.yaml` and `local_costmap_params.yaml` as plugins. [Click here](https://wiki.ros.org/navigation/Tutorials/RobotSetup#Global_Configuration) for more information on the difference between global and local costmaps. {% tabs %} {% tab title="global\_costmap\_params.yaml" %} ```yaml plugins: - {name: static_layer, type: "costmap_2d::StaticLayer"} - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: lowbstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: dynamic_obstacle_layer, type: "dynamic_obstacle_layer::DynamicLayer"} # Uncomment to apply dynamic_obstacle_layer - {name: inflation_layer, type: "costmap_2d::InflationLayer"} ``` {% endtab %} {% tab title="local\_costmap\_params.yaml" %} ```yaml plugins: - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: lowbstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"} # - {name: range_sensor_layer, type: "range_sensor_layer::RangeSensorLayer"} ``` {% endtab %} {% endtabs %} {% hint style="info" %} The most important thing in this section is to get the robot's footprint right. **Tip:** Tune the `teb_local_planner` params first. Modify the costmap only if you need more fine tuning after that. {% endhint %} #### 4.1. Inflation Layer This layer inflates the margin around lethal obstacles and specifies how much bigger you want to inflate the obstacles by. ```yaml inflation_layer: enabled: true cost_scaling_factor: 6.0 #added in by John inflation_radius: 0.39 #0.45 #Minimum value: 0.379 ``` `inflation_radius` is the radius, in meters, to which the map inflates obstacle cost value. Usually it is the width of the bot, plus some extra space. `cost_scaling_factors` is the scaling factor to apply to cost values during inflation. Additional information to configure it correctly (directly lifted from [here](https://answers.ros.org/question/12874/costmap2d-inflation-radius/?answer=19004#post-id-19004)) > The `inflation_radius` is actually the radius to which the cost scaling function is applied, not a parameter of the cost scaling function. Inside the inflation radius, the cost scaling function is applied, but outside the inflation radius, the cost of a cell is not inflated using the cost function. > > You'll have to make sure to set the inflation radius large enough that it includes the distance you need the cost function to be applied out to, as anything outside the inflation\_radius will not have the cost function applied. > > For the correct `cost_scaling_factor`, solve the equation there *( exp(-1.0 \* cost\_scaling\_factor \* (distance\_from\_obstacle - inscribed\_radius)) \* (costmap\_2d::INSCRIBED\_INFLATED\_OBSTACLE - 1))*, using your distance from obstacle and the cost value you want that cell to have. Ideally, we want to set these two parameters such that the inflation layer almost covers the corridors. And the robot is moving in the center between the obstacles. (See figure below) ![Image from https://kaiyuzheng.me/documents/navguide.pdf (Pg 12)](/files/-MlOSMNM6YAjy_PACWvj) #### 4.2. Obstacle Layer The obstacle layer marks out obstacles on the costmap. It tracks the obstacles as registered by sensor data. ```yaml obstacle_layer: origin_z: -0.1 z_resolution: 1.8 #1.5 This must be higher than the z coordinate of the mounted lidar z_voxels: 1 obstacle_range: 10.0 #10.0 raytrace_range: 15.0 #15.0 observation_sources: laser_scan_sensor track_unknown_space: true lethal_cost_threshold: 100 unknown_cost_value: 255 laser_scan_sensor: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, min_obstacle_height: 0.00, max_obstacle_height: 3.00} #point_cloud_sensor: {sensor_frame: lslidar_c16_frame, data_type: PointCloud2, topic: /lslidar_c16/lslidar_point_cloud, marking: true, clearing: true} ``` For 2D mapping, `laser_scan_sensor` must be selected. `obstacle_range` is the maximum distance from the robot that an obstacle will be inserted into the costmap. A value of 10 means the costmap will mark out obstacles that are within 10 meters from the robot. `raytrace_range` is the range in meters at which to raytrace out obstacles. The value must be set with respects to your sensors. `max_obstacle_height` is the maximum height of obstacles to be added to the costmap. Increase this number if you have very tall obstacles. The value must be set with respects to your sensors. *Voxel layer parameters* Voxels are 3D cubes that has a relative position in space. Can be used for 3D reconstruction with depth cameras. Ignore the parameters below if you are not using 3D. More information [here](). `origin_z` is the z-origin of the map (meters) `z_resolution` is the height of the cube > `z_resolution` controls how dense the voxels is on the z-axis. If it is higher, the voxel layers are denser. If the value is too low (e.g. 0.01), you won’t get useful costmap information. If you set z resolution to a higher value, your intention should be to obtain obstacles better, therefore you need to increase `z_voxels` parameter which controls how many voxels in each vertical column. It is also useless if you have too many voxels in a column but not enough resolution, because each vertical column has a limit in height. `z_voxels` is the number of voxels #### 4.3. lowbstacle layer Low obstacle layer is obstacle layer, but with additional parameters. Change your `observation_sources` to the topic that you want to take in data from. Use `obs_cloud` if you want a PointCloud (3D) or `mock_scan` if you want a LaserScan (2D). Or you can specify your own method. ```yaml lowbstacle_layer: origin_z: -0.1 z_resolution: 1.8 z_voxels: 1 obstacle_range: 3.5 #if beyond this threshold, then will not mark as obstacle raytrace_range: 5.0 #5.0 Lower this value to detect nearer obstacles with better accuracy observation_sources: obs_cloud mock_scan #butt_scan1 butt_scan2 publish_voxel_map: true track_unknown_space: true lethal_cost_threshold: 100 unknown_cost_value: 255 obs_cloud: data_type: PointCloud2 topic: /obstacles_cloud marking: true clearing: true min_obstacle_height: 0.01 max_obstacle_height: 0.99 mock_scan: data_type: LaserScan topic: /obstacles_scan marking: false clearing: true min_obstacle_height: 0.00 max_obstacle_height: 1.00 inf_is_valid: true ``` There are only two important parameters to note. `min_obstacle_height` is the height of the obstacle below the base of the robot. Examples includes stairs. Specify this param so that the robot can detect obstacles below its base link and prevent it from falling into the pit... ![This is the reason why we need a min\_obstacle\_height.](/files/-MlOYOTEdiBetVMmonro) `max_obstacle_height` is the maximum height of obstacle. It is usually specified as the height of the robot. ### 5. Uncomfortable situations You WILL definitely be encountering some of the issues. Because param tuning requires experimentation, it is suggested you test out your values in the console first before modifying the yaml files. `rosrun rqt_reconfigure rqt_reconfigure` {% hint style="info" %} List of known issues and possible fixes is not exhaustive. There will be many more issues yet to be encountered when extensively using the robot. Thus it is good practice to keep a list of problems and solutions so you know how to deal with the problem should it come up again. {% endhint %} #### **5.1. Some known uncomfortable situations** **Issue #1:** Problems getting the robot to go to the other side of the narrow path. It is noted from observations that the robot will be returning goal failure, or that robot will get itself uncomfortably close to walls and get stuck there. **Issue #2:** Problems getting the robot to turn 90 degrees. The planner somehow disregards the wall and direct the robot to go through it instead of making a 90 degree turn around the wall. Though it seems most robots are smart enough to recognize they cannot go through walls and will either abort the goal or get stuck trying. #### **5.2. Some attempted fix** **Solution #1:** Reducing the velocity and acceleration. The intuition is that by slowing down the movement and rotation of the robot, the planner might have more time to react to the obstacle and plan accordingly. **Solution #2:** (For robots that refuse to go to the other side of the corridor.) Shrink the `inflation_radius` in both costmaps so that it does not cover the corridor. (Right figure) Noted with observation that if the radius covers the entire corridor, robot may refuse to move. ![left: inflation\_radius covering corridor; robot refuses to move](/files/DUD2s5tAJ6k6bp3OFENp) ![right: cleared inflation\_radius](/files/QbxFaojgMSx2Vzd8bQKc) {% hint style="info" %} Check that the robot's footprint size is correct and check that the costmaps aren't blocking the paths. {% endhint %} ## References External resources if you need more information about planners and tuning planners. * [Navigation Tuning Guide - ROS wiki](http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide) * [ROS Navigation Tuning Guide](https://kaiyuzheng.me/documents/navguide.pdf) --- # Agent Instructions: Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://docs.movel.ai/new-collection/seirios-rns/documentation/teb-tuning-guide.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.