Issue1 moveit plugin crash

Fixed issue #1