重规划时，从当前位置初始化轨迹，速度解算异常

[zixie007]

ego-planner/src/planner/plan_manage/src/planner_manager.cpp

Lines 132 to 149 in bfda512

	else // Initial path generated from previous trajectory.
	{
	double t;
	double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();
	vector<double> pseudo_arc_length;
	vector<Eigen::Vector3d> segment_point;
	pseudo_arc_length.push_back(0.0);
	for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)
	{
	segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));
	if (t > t_cur)
	{
	pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());
	}
	}
	t -= ts;

• 源码从t_cur（结合规划频率可知 t_cur=1s）继承上段轨迹进行轨迹初始化。我想从无人机当前odom位置开始继承，通过遍历local_data_.duration_总能找到一个时间点（离当前无人机实际位置足够近），以该时间点代替t_cur实现继承。这样操作在1m/s的最大速度 1m/s^2最大加速度下可以正常运行。

• 但提高最大速度（2m/s）和最大加速度(5m/s^2)会出现问题。无人机向起点正方向航点规划速度解算没有问题，但给返航航点时(这里控制部分做了处理:要求先转向再追踪轨迹)，traj_server中解算的速度在前十几秒非常慢(几乎为-0.01~ -0.2)，更奇怪的是甚至解算出来的期望速度偶尔还有正值（0.1m/s类似的），过了十几秒后，速度解算正常。

• 我怀疑是时间分配的问题，但目前还没有找到问题所在。

[bigsuperZZZX]

🤔🤔🤔