В чем конкретно проблема?
- обнаружение свободного пути после вращения?
- Поворот к нему и вождение в него?
- Что-то еще?
Есть ли причина, по которой вы не можете использовать глобальный и локальный планировщик ROS и просто рассчитать для них цель после поворота?
Или вы имеете в виду, что ваш робот может двигаться только по прямойи вращаться на месте?Нет кривых вождения?
Чем взять глобальный план и проверить для каждой ячейки после старта, достижима ли она по прямой линии без столкновения с препятствием.Если нет, двигайтесь к предыдущей достижимой ячейке, поверните туда и начните заново.