Fix
Add check for planning scene monitor connection, with 5 sec delay