fixed issue #1, moveit plugin crashing
removed TODO from corrected code
Merge pull request #2 from ros-industrial/issue1_moveit_plugin_crash

Issue1 moveit plugin crash