Trajectory generator and parameter publisher implemnted and working