Update KDL SegmentMap interface to use shared pointers

This is dupe of https://github.com/ros/robot_state_publisher/pull/5 but the for groovy-devel branch