Change to use ros interface
fixed segmentation fault
Added message files