george

Not so sure the problem stems from `idx_replan` per se. I think the problem comes into play at line 138 `Eigen::MatrixXf replan_position = poly_diff(8, 0, (ros::Time::now().toSec() - time_start_replan.toSec() + planner_delay.toSec())/time(idx_replan))...