started to add Cmd Service to node
added UART write function
Added more commands
Merged with Urs branch. Things seemed to be broken.
Added sysid and compid to ros_mavlink.