Look at my ros package for the waist demo