ROS Code for Pathfinder