apollo自动驾驶进阶学习之：dreamview模块 sim_control原理
发布时间
阅读量：
阅读量
simcontrol作为dreamview模块的重要功能，能够在不使用仿真器的情况下模拟车辆运行并辅助planning模块进行轨迹规划与控制。该模块通过获取planning模块发送的导航、感知等信息为自动驾驶车辆规划运动轨迹，并将这些信息传递给控制模块以实现精确行驶。在开启simcontrol功能后无需单独开启control模块，因为sim_control会按照planning模块规划的轨迹直接执行行驶操作。此外，该功能还能够处理停车信息，并在必要时冻结车辆状态以确保安全运行。代码部分展示了如何根据当前时间匹配轨迹点并插值计算当前点坐标的具体实现过程。
1、引言
sim_control可被视作dreamview模块的关键组成部分，在功能实现上具有重要价值。该系统有助于我们对测试规划模块功能进行评估，在无需依赖于主流仿真平台（如lgsvl和carla）的情况下完成仿真实验，并模拟车辆运行的行为模式。
Planning系统基于 routing（导航模块）以及 prediction（感知模块）获取周围环境数据，并结合地图定位导航信息进行运算处理以确定自动驾驶车辆的具体行驶路径。随后系统将计算所得的数据结果发送至控制系统以便后续操作执行。
一旦开启sim_control功能，则无需启动control模块。因为其运行轨迹完全由planning模块规划。从而将测试终点设置为planning模块调试范围内。
2、流程介绍
3、代码
bool SimControl::PerfectControlModel(TrajectoryPoint* point,
Chassis::GearPosition* gear_position) {
// Result of the interpolation.
auto current_time = Clock::NowInSeconds();
//获取当前时间
const auto& trajectory = current_trajectory_->trajectory_point();
//获取planning模块发送的轨迹点
*gear_position = current_trajectory_->gear();
//获取planning模块发送的档位信息
if (!received_planning_) {
prev_point_ = next_point_;
} else {
//查看轨迹信息中是否有停车信息，如果有，则将车辆位置点定位到轨迹终点
if (current_trajectory_->estop().is_estop() ||
next_point_index_ >= trajectory.size()) {
// Freeze the car when there's an estop or the current trajectory has
// been exhausted.
Freeze();
} else {
// Determine the status of the car based on received planning message.
//如果没有停车信息，遍历整个轨迹中的点，查找时间与当前时间current_time 匹配的轨迹点
while (next_point_index_ < trajectory.size() &&
current_time > trajectory.Get(next_point_index_).relative_time() +
current_trajectory_->header().timestamp_sec()) {
++next_point_index_;
}
//如果index超过轨迹总个数，则取轨迹的最后一个点
if (next_point_index_ >= trajectory.size()) {
next_point_index_ = trajectory.size() - 1;
}
if (next_point_index_ == 0) {
AERROR << "First trajectory point is a future point!";
return false;
}
prev_point_index_ = next_point_index_ - 1;
//获取时间匹配的前后两个点
next_point_ = trajectory.Get(next_point_index_);
prev_point_ = trajectory.Get(prev_point_index_);
}
}
//如果时间大于后一个点，则直接取后一个点
if (current_time > next_point_.relative_time() +
current_trajectory_->header().timestamp_sec()) {
// Don't try to extrapolate if relative_time passes last point
*point = next_point_;
} else {
//否则用取前后两个线性差值的方式，获取当前时间所在点的信息。
*point = InterpolateUsingLinearApproximation(
prev_point_, next_point_,
current_time - current_trajectory_->header().timestamp_sec());
}
return true;
}全部评论 (0)
还没有任何评论哟~