now add nodes when robot looses wall, implemented pose controller for go_straigh