# Optimize Teb_planner

### 0. First try
{%youtube 7QmwzT5z3GY %}

{%youtube tWmSf36yvtk %}

---
### 1. Fix the bug (got stucked in a narrow hallway)
==Test #1==

> [color=#C0C0C0]base_local_planner_parameters_teb.yaml
> ```javascript=
> min_obstacle_dist: 0.2 #0.4
> inflation_dist: 0.3 #0.6
> dynamic_obstacle_inflation_dist: 0.3 #0.6
> ```

{%youtube zd7IwCFnifI %}
{%youtube j9mcKZYIp0c %}

---
### 2. Optimize (turned too much and too quick)
==Test #2==

> [color=#C0C0C0]base_local_planner_parameters_teb.yaml
> ```javascript=
> max_vel_theta: 1.0 #2.75
> acc_lim_x: 1.0 #2.5
> acc_lim_theta: 0.6 #3.2
> ```

{%youtube Vj7nV_Rcu_E %}

---
### 3. Optimize and display the odom path
> [color=#C0C0C0]base_local_planner_parameters_teb.yaml
> ```javascript=
> max_vel_x: 0.18 #0.2
> max_vel_x_backwards: 0.18 #0.2
> max_vel_theta: 0.13 #1.0
> acc_lim_theta: 0.43 #0.6
> ```

==Test #3 / Untuned :point_left: / :point_right:Tuned==
 

 

---
### 4. Add loop function for testing
`roslaunch indicate_tag goal_loop.launch`
:::danger
:bulb: **Bug：** teb_planner sometimes got stucked at the end points
:::
 

---
### 5. Reconfigure costmap parameters

==Test #4==

> [color=#C0C0C0]costmap_common_parameters_burger.yaml
> ```javascript=
> inflation_radius: 1.2 #1.0
> cost_scaling_factor: 4.0 #3.0
> ```

> [color=#C0C0C0]base_local_planner_parameters_teb.yaml
> ```javascript=
> max_global_plan_lookahead_dist: 3.5 #3.0
> yaw_goal_tolerance: 0.12 #0.17
> min_obstacle_dist: 0.15 #0.2
> inflation_dist: 0.2 #0.3
> dynamic_obstacle_inflation_dist: 0.2 #0.3
> global_plan_viapoint_sep: 0.03 #add
> ```

> [color=#C0C0C0]move_base_params.yaml
> ```javascript=
> max_planning_retries: 3.0 #add
> ```

---
### 6. [Add costmap_converter plugins](https://wiki.ros.org/teb_local_planner/Tutorials/Costmap%20conversion)

==Test #5==
` costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"`

:::success
:bulb: **Solve：** Use costmap converter plugins to decrease **computational times and instabilities in the calculation of distinctive topologies**.
>[color=#AAAAAA]推測是由於使用costmap converter,減少計算時間,control loop與map update才可以跟上預設的更新頻率,警號訊號消失!且大幅減少車子被困住的情況(目前還不知道是什麼原因造成的).
 **<- Befor plugins**
 **<- After plugins**
:::

---
### 7. Reference

##### 7-1. [ROS wiki- Teb_planner tutorials](http://wiki.ros.org/teb_local_planner/Tutorials)