Updated code, working in gazebo