@moriarty This test_PDST follows new_planner style. It works fine. This reference to #51

But i don't know why it doesn't stops after publishing the path. It keeps doing again and agian.