Added vision code, got the main code compiling and running moveit.