+ next_ctrl->set_control("speed", 0);
+ }
+ else if((!approach && rsv_dist<brake_dist*1.3+approach_margin) || (blocked && rsv_dist>brake_dist+margin*2))
+ {
+ blocked = false;
+ approach = true;
+ if(target_speed.value>approach_speed)
+ next_ctrl->set_control("speed", approach_speed);
+ else
+ next_ctrl->set_control("speed", target_speed.value);