I've looked through the code and I think it would be great to see this merged into the experimental package! Perhaps open a Github pull request and we can continue discussion there? --- [Visit Topic](https://discourse.ros.org/t/a-real-time-motion-planner/4354/4) or reply to this email to respond.