Replan init state

[Chortine]

Hi Team,

In replan_fsm.cpp, in the main loop, when the exec_state_ is REPLAN_TRAJ, the init state of the planner is set to the state on the previously planned trajectory at current time https://github.com/ZJU-FAST-Lab/Car-like-Robotic-swarm/blob/eabbf7c907babe450b49da88c204bf61bf6c1709/src/Minco/traj_planner/src/replan_fsm.cpp#L164C1-L165C1. Why it is like this? In this case, even the car never executed the command, the planned trajectory will not be effected and will be further and further from the car's true pose. Why the replan init state is not the true pose of the car at current time?

Thanks.

[Leon-Redfield]

This is a good question. We select the state on the previously planned trajectory is because the planning module need some time to compute, for instance, 35ms. And we have a time budget in this project, 60ms. So ideally, we want the exact state on the trajectory 35ms after the current time to be the initial state, but the computation of each replan is not certain. So we set a time budget of 60ms which is more than general planning time. The main reason why we don't use current time is because we want to guarantee the consistency of planning, which means the trajectories of last planned and replanned can be smoothly spliced at the replanned point. And there is another reason, in general planning task, there is a controller to follow the planned trajectory. If we select current time and state as the initial state and time, after planning, the time is 35ms later, during the 35ms, the controller is following the previously planned trajectory, while the new trajectory is not consistent with the previous trajectory. So when the new trajectory is generated, the controller has to be shifted to follow the new trajectory, causing a little inconsistency. This is a little bit abstract, hope you can understand it. In real world experiments, to be honest, the time budget doesn't affect too much because the controller is robust to a little inconsistency.