Apollo2.0自动驾驶之apollo/modules/common/vehicle_state/vehicle_state_provider.cc

/****************Apollo源码分析****************************

Copyright 2018 The File Authors & zouyu. All Rights Reserved.

Contact with: 1746430162@qq.com 181663309504

源码主要是c++实现的，也有少量python，git下载几百兆，其实代码不太多，主要是地图和数据了大量空间，主要程序
在apollo/modules目录中，

我们把它分成以下几部分（具体说明见各目录下的modules）：

感知：感知当前位置，速度，障碍物等等

Apollo/modules/perception

预测：对场景下一步的变化做出预测

Apollo/modules/prediction

规划：

(1) 全局路径规划：通过起点终点计算行驶路径

Apollo/modules/routing

(2) 规划当前轨道：通过感知，预测，路径规划等信息计算轨道

Apollo/modules/planning

(3) 规划转换成命令：将轨道转换成控制汽车的命令（加速，制动，转向等）

Apollo/modules/control

其它

(1) 输入输出

i. Apollo/modules/drivers 设备驱动

ii. Apollo/modules/localization 位置信息

iii. Apollo/modules/monitor 监控模块

iv. Apollo/modules/canbus 与汽车硬件交互

v. Apollo/modules/map 地图数据

vi. Apollo/modules/third_party_perception 三方感知器支持

(2) 交互

i. Apollo/modules/dreamview 可视化模块

ii. Apollo/modules/hmi 把汽车当前状态显示给用户

(3) 工具

i. Apollo/modules/calibration 标注工具

ii. Apollo/modules/common 支持其它模块的公共工具

iii. Apollo/modules/data 数据工具

iv. Apollo/modules/tools 一些Python工具

(4) 其它

i. Apollo/modules/elo 高精度定位系统，无源码，但有文档

ii. Apollo/modules/e2e 收集传感器数据给PX2,ROS

  自动驾驶系统先通过起点终点规划出整体路径（routing）；然后在行驶过程中感知（perception）当前环境
（识别车辆行人路况标志等），并预测下一步发展；然后把已知信息都传入规划模块（planning），规划出之后的轨道；
控制模块（control）将轨道数据转换成对车辆的控制信号，通过汽车交互模块（canbus）控制汽车．

我觉得这里面算法技术含量最高的是感知perception和规划planning，具体请见本博客中各模块的分析代码。

/****************************************************************************************

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/common/vehicle_state/vehicle_state_provider.h"

#include <cmath>

#include "Eigen/Core"

#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/math/euler_angles_zxy.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/string_util.h"
#include "modules/localization/common/localization_gflags.h"

namespace apollo {
namespace common {

VehicleStateProvider::VehicleStateProvider() {}

Status VehicleStateProvider::Update(
    const localization::LocalizationEstimate &localization,
    const canbus::Chassis &chassis) {
  original_localization_ = localization;
  if (!ConstructExceptLinearVelocity(localization)) {
    std::string msg = util::StrCat(
        "Fail to update because ConstructExceptLinearVelocity error.",
        "localization:\n", localization.DebugString());
    return Status(ErrorCode::LOCALIZATION_ERROR, msg);
  }
  if (localization.has_header() && localization.header().has_timestamp_sec()) {
    vehicle_state_.set_timestamp(localization.header().timestamp_sec());
  } else if (chassis.has_header() && chassis.header().has_timestamp_sec()) {
    AERROR << "Unable to use location timestamp for vehicle state. Use chassis "
              "time instead.";
    vehicle_state_.set_timestamp(chassis.header().timestamp_sec());
  }

  if (chassis.has_speed_mps()) {
    vehicle_state_.set_linear_velocity(chassis.speed_mps());
  }

  if (chassis.has_gear_location()) {
    vehicle_state_.set_gear(chassis.gear_location());
  } else {
    vehicle_state_.set_gear(canbus::Chassis::GEAR_NONE);
  }
  vehicle_state_.set_driving_mode(chassis.driving_mode());

  return Status::OK();
}

bool VehicleStateProvider::ConstructExceptLinearVelocity(
    const localization::LocalizationEstimate &localization) {
  if (!localization.has_pose()) {
    AERROR << "Invalid localization input.";
    return false;
  }
  // skip localization update when it is in use_navigation_mode.
  if (FLAGS_use_navigation_mode) {
    ADEBUG << "Skip localization update when it is in use_navigation_mode.";
    return true;
  }

  vehicle_state_.mutable_pose()->CopyFrom(localization.pose());
  if (localization.pose().has_position()) {
    vehicle_state_.set_x(localization.pose().position().x());
    vehicle_state_.set_y(localization.pose().position().y());
    vehicle_state_.set_z(localization.pose().position().z());
  }

  const auto &orientation = localization.pose().orientation();

  if (localization.pose().has_heading()) {
    vehicle_state_.set_heading(localization.pose().heading());
  } else {
    vehicle_state_.set_heading(
        math::QuaternionToHeading(orientation.qw(), orientation.qx(),
                                  orientation.qy(), orientation.qz()));
  }

  if (FLAGS_enable_map_reference_unify) {
    if (!localization.pose().has_angular_velocity_vrf()) {
      AERROR << "localization.pose().has_angular_velocity_vrf() must be true "
                "when FLAGS_enable_map_reference_unify is true.";
      return false;
    }
    vehicle_state_.set_angular_velocity(
        localization.pose().angular_velocity_vrf().z());

    if (!localization.pose().has_linear_acceleration_vrf()) {
      AERROR << "localization.pose().has_linear_acceleration_vrf() must be "
                "true when FLAGS_enable_map_reference_unify is true.";
      return false;
    }
    vehicle_state_.set_linear_acceleration(
        localization.pose().linear_acceleration_vrf().y());
  } else {
    CHECK(localization.pose().has_angular_velocity());
    vehicle_state_.set_angular_velocity(
        localization.pose().angular_velocity().z());
    CHECK(localization.pose().has_linear_acceleration());
    vehicle_state_.set_linear_acceleration(
        localization.pose().linear_acceleration().y());
  }

  if (!(vehicle_state_.linear_velocity() > 0.0)) {
    vehicle_state_.set_kappa(0.0);
  } else {
    vehicle_state_.set_kappa(vehicle_state_.angular_velocity() /
                             vehicle_state_.linear_velocity());
  }

  if (localization.pose().has_euler_angles()) {
    vehicle_state_.set_roll(localization.pose().euler_angles().x());
    vehicle_state_.set_pitch(localization.pose().euler_angles().y());
    vehicle_state_.set_yaw(localization.pose().euler_angles().z());
  } else {
    math::EulerAnglesZXYd euler_angle(orientation.qw(), orientation.qx(),
                                      orientation.qy(), orientation.qz());
    vehicle_state_.set_roll(euler_angle.roll());
    vehicle_state_.set_pitch(euler_angle.pitch());
    vehicle_state_.set_yaw(euler_angle.yaw());
  }

  return true;
}

double VehicleStateProvider::x() const { return vehicle_state_.x(); }

double VehicleStateProvider::y() const { return vehicle_state_.y(); }

double VehicleStateProvider::z() const { return vehicle_state_.z(); }

double VehicleStateProvider::roll() const { return vehicle_state_.roll(); }

double VehicleStateProvider::pitch() const { return vehicle_state_.pitch(); }

double VehicleStateProvider::yaw() const { return vehicle_state_.yaw(); }

double VehicleStateProvider::heading() const {
  return vehicle_state_.heading();
}

double VehicleStateProvider::kappa() const { return vehicle_state_.kappa(); }

double VehicleStateProvider::linear_velocity() const {
  return vehicle_state_.linear_velocity();
}

double VehicleStateProvider::angular_velocity() const {
  return vehicle_state_.angular_velocity();
}

double VehicleStateProvider::linear_acceleration() const {
  return vehicle_state_.linear_acceleration();
}

double VehicleStateProvider::gear() const { return vehicle_state_.gear(); }

double VehicleStateProvider::timestamp() const {
  return vehicle_state_.timestamp();
}

const localization::Pose &VehicleStateProvider::pose() const {
  return vehicle_state_.pose();
}

const localization::Pose &VehicleStateProvider::original_pose() const {
  return original_localization_.pose();
}

void VehicleStateProvider::set_linear_velocity(const double linear_velocity) {
  vehicle_state_.set_linear_velocity(linear_velocity);
}

const VehicleState &VehicleStateProvider::vehicle_state() const {
  return vehicle_state_;
}

math::Vec2d VehicleStateProvider::EstimateFuturePosition(const double t) const {
  Eigen::Vector3d vec_distance(0.0, 0.0, 0.0);
  double v = vehicle_state_.linear_velocity();
  if (vehicle_state_.gear() == canbus::Chassis::GEAR_REVERSE) {
    v = -vehicle_state_.linear_velocity();
  }
  // Predict distance travel vector
  if (std::fabs(vehicle_state_.angular_velocity()) < 0.0001) {
    vec_distance[0] = 0.0;
    vec_distance[1] = v * t;
  } else {
    vec_distance[0] = -v / vehicle_state_.angular_velocity() *
                      (1.0 - std::cos(vehicle_state_.angular_velocity() * t));
    vec_distance[1] = std::sin(vehicle_state_.angular_velocity() * t) * v /
                      vehicle_state_.angular_velocity();
  }

  // If we have rotation information, take it into consideration.
  if (vehicle_state_.pose().has_orientation()) {
    const auto &orientation = vehicle_state_.pose().orientation();
    Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
                                         orientation.qy(), orientation.qz());
    Eigen::Vector3d pos_vec(vehicle_state_.x(), vehicle_state_.y(),
                            vehicle_state_.z());
    auto future_pos_3d = quaternion.toRotationMatrix() * vec_distance + pos_vec;
    return math::Vec2d(future_pos_3d[0], future_pos_3d[1]);
  }

  // If no valid rotation information provided from localization,
  // return the estimated future position without rotation.
  return math::Vec2d(vec_distance[0] + vehicle_state_.x(),
                     vec_distance[1] + vehicle_state_.y());
}

math::Vec2d VehicleStateProvider::ComputeCOMPosition(
    const double rear_to_com_distance) const {
  // set length as distance between rear wheel and center of mass.
  Eigen::Vector3d v(0.0, rear_to_com_distance, 0.0);
  Eigen::Vector3d pos_vec(vehicle_state_.x(), vehicle_state_.y(),
                          vehicle_state_.z());
  // Initialize the COM position without rotation
  Eigen::Vector3d com_pos_3d = v + pos_vec;

  // If we have rotation information, take it into consideration.
  if (vehicle_state_.pose().has_orientation()) {
    const auto &orientation = vehicle_state_.pose().orientation();
    Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
                                         orientation.qy(), orientation.qz());
    // Update the COM position with rotation
    com_pos_3d = quaternion.toRotationMatrix() * v + pos_vec;
  }
  return math::Vec2d(com_pos_3d[0], com_pos_3d[1]);
}

}  // namespace common
}  // namespace apollo

/******************************************************************************

* Copyright 2017 The Apollo Authors. All Rights Reserved.

*

* Licensed under the Apache License, Version 2.0 (the "License");

* you may not use this file except in compliance with the License.

* You may obtain a copy of the License at

*

* http://www.apache.org/licenses/LICENSE-2.0

*

* Unless required by applicable law or agreed to in writing, software

* distributed under the License is distributed on an "AS IS" BASIS,

* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

* See the License for the specific language governing permissions and

* limitations under the License.

*****************************************************************************/

#include "modules/common/vehicle_state/vehicle_state_provider.h"

#include <cmath>

#include "Eigen/Core"

#include "modules/common/configs/config_gflags.h"

#include "modules/common/log.h"

#include "modules/common/math/euler_angles_zxy.h"

#include "modules/common/math/quaternion.h"

#include "modules/common/util/string_util.h"

#include "modules/localization/common/localization_gflags.h"

namespace apollo {

namespace common {

VehicleStateProvider::VehicleStateProvider () {}

Status VehicleStateProvider::Update ( //车辆状态更新

const localization::LocalizationEstimate & localization, //定义了一个位置估计的常量

const canbus::Chassis & chassis) { //底盘数据接口的常量

original_localization_ = localization; //把现在当前的位置给“original_localization_”，更新位置信息。

if ( ! ConstructExceptLinearVelocity (localization)) {

std::string msg = util::StrCat ( //如果定位出现异常，打印出错信息

"Fail to update because ConstructExceptLinearVelocity error." ,

"localization: \n " , localization. DebugString ());

return Status (ErrorCode::LOCALIZATION_ERROR, msg); //返回出错状态消息

}

if (localization. has_header () && localization. header (). has_timestamp_sec ()) {

vehicle_state_. set_timestamp (localization. header (). timestamp_sec ());

} else if (chassis. has_header () && chassis. header (). has_timestamp_sec ()) {

AERROR << "Unable to use location timestamp for vehicle state. Use chassis "

"time instead." ; //以时间为基准，返回车辆头部信息

vehicle_state_. set_timestamp (chassis. header (). timestamp_sec ());

}

if (chassis. has_speed_mps ()) {

vehicle_state_. set_linear_velocity (chassis. speed_mps ());

}

if (chassis. has_gear_location ()) { //返回底盘齿轮位置，这是自动控制输入环节比较重要的参数

vehicle_state_. set_gear (chassis. gear_location ());

} else {

vehicle_state_. set_gear (canbus::Chassis::GEAR_NONE);

}

vehicle_state_. set_driving_mode (chassis. driving_mode ()); //设置底盘驱动模式

return Status::OK ();

}

bool VehicleStateProvider::ConstructExceptLinearVelocity (

const localization::LocalizationEstimate & localization) {

if ( ! localization. has_pose ()) {

AERROR << "Invalid localization input." ;

return false ;

}

// skip localization update when it is in use_navigation_mode.

if (FLAGS_use_navigation_mode) {

ADEBUG << "Skip localization update when it is in use_navigation_mode." ;

return true ;

}

vehicle_state_. mutable_pose ()-> CopyFrom (localization. pose ());

if (localization. pose (). has_position ()) {

vehicle_state_. set_x (localization. pose (). position (). x ()); //车辆定位的位置姿态：x y z

vehicle_state_. set_y (localization. pose (). position (). y ());

vehicle_state_. set_z (localization. pose (). position (). z ());

}

const auto & orientation = localization. pose (). orientation ();

if (localization. pose (). has_heading ()) {

vehicle_state_. set_heading (localization. pose (). heading ());

} else {

vehicle_state_. set_heading (

math::QuaternionToHeading (orientation. qw (), orientation. qx (),

orientation. qy (), orientation. qz ()));

}

if (FLAGS_enable_map_reference_unify) {

if ( ! localization. pose (). has_angular_velocity_vrf ()) {

AERROR << "localization.pose().has_angular_velocity_vrf() must be true "

"when FLAGS_enable_map_reference_unify is true." ;

return false ;

}

vehicle_state_. set_angular_velocity (

localization. pose (). angular_velocity_vrf (). z ());

if ( ! localization. pose (). has_linear_acceleration_vrf ()) { //线性加速

AERROR << "localization.pose().has_linear_acceleration_vrf() must be "

"true when FLAGS_enable_map_reference_unify is true." ;

return false ;

}

vehicle_state_. set_linear_acceleration (

localization. pose (). linear_acceleration_vrf (). y ());

} else {

CHECK (localization. pose (). has_angular_velocity ());

vehicle_state_. set_angular_velocity (

localization. pose (). angular_velocity (). z ());

CHECK (localization. pose (). has_linear_acceleration ());

vehicle_state_. set_linear_acceleration (

localization. pose (). linear_acceleration (). y ());

}

if ( ! (vehicle_state_. linear_velocity () > 0.0 )) {

vehicle_state_. set_kappa ( 0.0 );

} else {

vehicle_state_. set_kappa (vehicle_state_. angular_velocity () /

vehicle_state_. linear_velocity ());

}

if (localization. pose (). has_euler_angles ()) { //车辆定位角度

vehicle_state_. set_roll (localization. pose (). euler_angles (). x ());

vehicle_state_. set_pitch (localization. pose (). euler_angles (). y ());

vehicle_state_. set_yaw (localization. pose (). euler_angles (). z ());

} else {

math::EulerAnglesZXYd euler_angle (orientation. qw (), orientation. qx (),

orientation. qy (), orientation. qz ());

vehicle_state_. set_roll (euler_angle. roll ()); //车辆姿态角

vehicle_state_. set_pitch (euler_angle. pitch ());

vehicle_state_. set_yaw (euler_angle. yaw ());

}

return true ;

}

double VehicleStateProvider::x () const { return vehicle_state_. x (); } //车辆各种状态的反馈

double VehicleStateProvider::y () const { return vehicle_state_. y (); }

double VehicleStateProvider::z () const { return vehicle_state_. z (); }

double VehicleStateProvider::roll () const { return vehicle_state_. roll (); }

double VehicleStateProvider::pitch () const { return vehicle_state_. pitch (); }

double VehicleStateProvider::yaw () const { return vehicle_state_. yaw (); }

double VehicleStateProvider::heading () const {

return vehicle_state_. heading ();

}

double VehicleStateProvider::kappa () const { return vehicle_state_. kappa (); }

double VehicleStateProvider::linear_velocity () const {

return vehicle_state_. linear_velocity ();

}

double VehicleStateProvider::angular_velocity () const {

return vehicle_state_. angular_velocity ();

}

double VehicleStateProvider::linear_acceleration () const {

return vehicle_state_. linear_acceleration ();

}

double VehicleStateProvider::gear () const { return vehicle_state_. gear (); }

double VehicleStateProvider::timestamp () const {

return vehicle_state_. timestamp ();

}

const localization::Pose & VehicleStateProvider::pose () const {

return vehicle_state_. pose ();

}

const localization::Pose & VehicleStateProvider::original_pose () const {

return original_localization_. pose ();

}

void VehicleStateProvider::set_linear_velocity ( const double linear_velocity) {

vehicle_state_. set_linear_velocity (linear_velocity);

}

const VehicleState & VehicleStateProvider::vehicle_state () const {

return vehicle_state_;

}

math::Vec2d VehicleStateProvider::EstimateFuturePosition ( const double t) const {

Eigen::Vector3d vec_distance ( 0.0 , 0.0 , 0.0 );

double v = vehicle_state_. linear_velocity ();

if (vehicle_state_. gear () == canbus::Chassis::GEAR_REVERSE) {

v = - vehicle_state_. linear_velocity ();

}

// Predict distance travel vector 预测行驶距离

if ( std::fabs (vehicle_state_. angular_velocity ()) < 0.0001 ) {

vec_distance[ 0 ] = 0.0 ;

vec_distance[ 1 ] = v * t;

} else {

vec_distance[ 0 ] = - v / vehicle_state_. angular_velocity () *

( 1.0 - std::cos (vehicle_state_. angular_velocity () * t));

vec_distance[ 1 ] = std::sin (vehicle_state_. angular_velocity () * t) * v /

vehicle_state_. angular_velocity ();

}

// If we have rotation information, take it into consideration.

if (vehicle_state_. pose (). has_orientation ()) {

const auto & orientation = vehicle_state_. pose (). orientation ();

Eigen::Quaternion < double > quaternion (orientation. qw (), orientation. qx (),

orientation. qy (), orientation. qz ());

Eigen::Vector3d pos_vec (vehicle_state_. x (), vehicle_state_. y (),

vehicle_state_. z ()); // Eigen是一个非常重要的矩阵库，相应的函数意义请自行查找

auto future_pos_3d = quaternion. toRotationMatrix () * vec_distance + pos_vec;

return math::Vec2d (future_pos_3d[ 0 ], future_pos_3d[ 1 ]);

}

// If no valid rotation information provided from localization,

// return the estimated future position without rotation.

return math::Vec2d (vec_distance[ 0 ] + vehicle_state_. x (),

vec_distance[ 1 ] + vehicle_state_. y ());

}

math::Vec2d VehicleStateProvider::ComputeCOMPosition (

const double rear_to_com_distance) const {

// set length as distance between rear wheel and center of mass.

Eigen::Vector3d v ( 0.0 , rear_to_com_distance, 0.0 );

Eigen::Vector3d pos_vec (vehicle_state_. x (), vehicle_state_. y (),

vehicle_state_. z ());

// Initialize the COM position without rotation

Eigen::Vector3d com_pos_3d = v + pos_vec;

// If we have rotation information, take it into consideration.

if (vehicle_state_. pose (). has_orientation ()) {

const auto & orientation = vehicle_state_. pose (). orientation ();

Eigen::Quaternion < double > quaternion (orientation. qw (), orientation. qx (),

orientation. qy (), orientation. qz ());

// Update the COM position with rotation

com_pos_3d = quaternion. toRotationMatrix () * v + pos_vec;

}

return math::Vec2d (com_pos_3d[ 0 ], com_pos_3d[ 1 ]);

}

} // namespace common

} // namespace apollo