提交 6c3ab442 编写于 作者: Z Zhang Liangliang 提交者: Jiangtao Hu

Planning: added dkappa in planning path.

上级 a2d7c8c3
......@@ -185,11 +185,14 @@ bool PathData::SLToXY(const FrenetFramePath &frenet_path,
if (path_points.empty()) {
path_point.set_s(0.0);
path_point.set_dkappa(0.0);
} else {
common::math::Vec2d last(path_points.back().x(), path_points.back().y());
common::math::Vec2d current(path_point.x(), path_point.y());
double distance = (last - current).Length();
path_point.set_s(path_points.back().s() + distance);
path_point.set_dkappa((path_point.kappa() - path_points.back().kappa()) /
distance);
}
path_points.push_back(std::move(path_point));
}
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册