PSM: Correctly handle full planning scene message

[sjahr]

Port #3610

Description

Please explain the changes you made, including a reference to the related issue if applicable

Checklist

• [ ] Required by CI: Code is auto formatted using clang-format
• [ ] Extend the tutorials / documentation reference
• [ ] Document API changes relevant to the user in the MIGRATION.md notes
• [ ] Create tests, which fail without this PR reference
• [ ] Include a screenshot if changing a GUI
• [ ] While waiting for someone to review your request, please help review another open pull request to support the maintainers