Algorithm A2 processObstalcesOnLocalTarget

Input:$local\_target=(x_{rp},y_{rp})$, $local\_costmap$, l, $\mathsf{\Delta }\theta$, $\mathsf{\Delta }l$, $\mathsf{\Delta }s$, $\mathsf{\Delta }\varphi$ Output:$new\_local\_target$ 1:Get the cost of local target from $local\_costmap\to C_{local}$ 2:if$C_{local}>0$then 3:    ${\mathit{V}}_{\mathit{dir}}=(v_x,v_y)$ 4:    $\theta \leftarrow arctan\frac{v_y}{v_x}$ 5:    $x_{gt}\leftarrow x_{lt}-lsin\theta$ 6:    $y_{gt}\leftarrow y_{lt}+lcos\theta$ 7:    ${\theta }_{gt}\leftarrow \theta +\mathsf{\Delta }\theta$ 8:    if $C_{gt}>0$ then 9:        $s\leftarrow Euclidean\_distance(robot,local\_target)$ 10:      $seti=1$ 11:      while $\mathsf{\Delta }\theta -i\mathsf{\Delta }\varphi >0$ and $l-i\mathsf{\Delta }l>0$ and $s-i\mathsf{\Delta }s>0$ do 12:         $x_{as}\leftarrow x_{gt}+i\mathsf{\Delta }lcos\left(\frac{\pi }{2}-\theta \right)$ 13:         $y_{as}\leftarrow y_{gt}-i\mathsf{\Delta }lsin\left(\frac{\pi }{2}-\theta \right)$ 14:         $x_{sp}\leftarrow x_{as}-i\mathsf{\Delta }scos\theta$ 15:         $y_{sp}\leftarrow y_{as}-i\mathsf{\Delta }ssin\theta$ 16:         ${\theta }_{sp}\leftarrow \theta +\mathsf{\Delta }\theta -i\mathsf{\Delta }\varphi$ 17:         if $C_{sp}==0$ then 18:          return $new\_local\_target=(x_{sp},y_{sp},{\theta }_{sp})$ 19:         end if 20:         $i=i+1$ 21:    end while 22:  else 23:     return $guided\_turning\_point=(x_{gt},y_{gt},{\theta }_{gt})$ 24:   end if 25:else 26:   return $local\_target=(x_{lt},y_{lt},{\theta }_{lt})$ 27: end if