In the code you already posted. Put some additional ones in for troubleshooting. Are you familiar with ROS? Time to learn, not in this thread though. However, just copy the ones that are there for now.