Hi @Alstrup, welcome to the forum 
I believe this comment should cover what’s going wrong, and how to fix it. If that doesn’t resolve it, please feel free to follow up with some more information of your current parameters and software versions, and what you’re ultimately trying to achieve.
As a side note, I’ve edited your post to use a code block, to make the code easier to follow and copy as relevant. Please see the How to Use the Blue Robotics Forums post for more information about what’s possible