This should be a simplified use case, for example, just moving Albert with PlanSys2 and adapting the speed. Check the following repo for an example: https://github.com/kas-lab/navigation_graph_map/tree/main