So, to summarize: You have a working driver, where you can send commands and receive status information?
In that case, wrapping ROS should be really easy. Here's what I usually do:
- Set up an empty c++ ROS package
- Put your code inside and compile it using ROS facilities (No ROS code yet, should basically spit out the same functionality you have now)
- Initialize ROS in you package and create a ROS loop. If you need some polling or so, use
ros::spinOnce()
along with your code - still same functionality. - Add status/position publishers for the status information. This should give you the same functionality + outgoing ROS msgs.
- Add subscribers/service calls for sending commands. This is the step, where you need to decide what's the best way to control this (probably messages here) and obviously disable your manual joystick control to not interfere.