[Navigation][Trajectory Generation] should use the last target pose or the current pose as the start point for the new trajectory

[tongtybj]

Current implementation is based on the last target pose:
tongtybj@c2fdf80

Do not know the reason why changed from the current pose to the last target pose.

[tongtybj]

solved by #709