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

/****************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.
 *****************************************************************************/
/**
 * @file vehicle_state.h
 *
 * @brief Declaration of the class VehicleStateProvider.
 */
#ifndef MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_PROVIDER_H_
#define MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_PROVIDER_H_

#include <memory>
#include <string>

#include "modules/canbus/proto/chassis.pb.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/localization/proto/localization.pb.h"

#include "modules/common/macro.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/status/status.h"

/**
 * @namespace apollo::common
 * @brief apollo::common
 */
namespace apollo {
namespace common {

/**
 * @class VehicleStateProvider
 * @brief The class of vehicle state.
 *        It includes basic information and computation
 *        about the state of the vehicle.
 */
class VehicleStateProvider {
 public:
  /**
   * @brief Constructor by information of localization and chassis.
   * @param localization Localization information of the vehicle.
   * @param chassis Chassis information of the vehicle.
   */
  Status Update(const localization::LocalizationEstimate& localization,
                const canbus::Chassis& chassis);

  /**
   * @brief Update VehicleStateProvider instance by protobuf files.
   * @param localization_file the localization protobuf file.
   * @param chassis_file The chassis protobuf file
   */
  void Update(const std::string& localization_file,
              const std::string& chassis_file);

  double timestamp() const;

  const localization::Pose& pose() const;
  const localization::Pose& original_pose() const;

  /**
   * @brief Default destructor.
   */
  virtual ~VehicleStateProvider() = default;

  /**
   * @brief Get the x-coordinate of vehicle position.
   * @return The x-coordinate of vehicle position.
   */
  double x() const;

  /**
   * @brief Get the y-coordinate of vehicle position.
   * @return The y-coordinate of vehicle position.
   */
  double y() const;

  /**
   * @brief Get the z coordinate of vehicle position.
   * @return The z coordinate of vehicle position.
   */
  double z() const;

  double kappa() const;

  /**
   * @brief Get the vehicle roll angle.
   * @return The euler roll angle.
   */
  double roll() const;

  /**
   * @brief Get the vehicle pitch angle.
   * @return The euler pitch angle.
   */
  double pitch() const;

  /**
   * @brief Get the vehicle yaw angle.
   *  As of now, use the heading instead of yaw angle.
   *  Heading angle with East as zero, yaw angle has North as zero
   * @return The euler yaw angle.
   */
  double yaw() const;

  /**
   * @brief Get the heading of vehicle position, which is the angle
   *        between the vehicle's heading direction and the x-axis.
   * @return The angle between the vehicle's heading direction
   *         and the x-axis.
   */
  double heading() const;

  /**
   * @brief Get the vehicle's linear velocity.
   * @return The vehicle's linear velocity.
   */
  double linear_velocity() const;

  /**
   * @brief Get the vehicle's angular velocity.
   * @return The vehicle's angular velocity.
   */
  double angular_velocity() const;

  /**
   * @brief Get the vehicle's linear acceleration.
   * @return The vehicle's linear acceleration.
   */
  double linear_acceleration() const;

  /**
   * @brief Get the vehicle's gear position.
   * @return The vehicle's gear position.
   */
  double gear() const;

  /**
   * @brief Set the vehicle's linear velocity.
   * @param linear_velocity The value to set the vehicle's linear velocity.
   */
  void set_linear_velocity(const double linear_velocity);

  /**
   * @brief Estimate future position from current position and heading,
   *        along a period of time, by constant linear velocity,
   *        linear acceleration, angular velocity.
   * @param t The length of time period.
   * @return The estimated future position in time t.
   */
  math::Vec2d EstimateFuturePosition(const double t) const;

  /**
   * @brief Compute the position of center of mass(COM) of the vehicle,
   *        given the distance from rear wheels to the center of mass.
   * @param rear_to_com_distance Distance from rear wheels to
   *        the vehicle's center of mass.
   * @return The position of the vehicle's center of mass.
   */
  math::Vec2d ComputeCOMPosition(const double rear_to_com_distance) const;

  const VehicleState& vehicle_state() const;

 private:
  bool ConstructExceptLinearVelocity(
      const localization::LocalizationEstimate& localization);

  common::VehicleState vehicle_state_;
  localization::LocalizationEstimate original_localization_;

  DECLARE_SINGLETON(VehicleStateProvider);
};

}  // namespace common
}  // namespace apollo

#endif  // MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_PROVIDER_H_

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

* 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.

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

/**

* @file vehicle_state.h

*

* @brief Declaration of the class VehicleStateProvider.

*/

#ifndef MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_PROVIDER_H_

#define MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_PROVIDER_H_

#include <memory>

#include <string>

#include "modules/canbus/proto/chassis.pb.h" //汽车底盘数据。

#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"

#include "modules/localization/proto/localization.pb.h" //定位数据

#include "modules/common/macro.h"

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

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

#include "modules/common/status/status.h"

/**

* @namespace apollo::common

* @brief apollo::common

*/

namespace apollo { //这个头文件给出了汽车底盘的各种状态数据接口，具有重要的意义。

namespace common {

/**

* @class VehicleStateProvider

* @brief The class of vehicle state.

* It includes basic information and computation

* about the state of the vehicle.

*/

class VehicleStateProvider {

public:

/**

* @brief Constructor by information of localization and chassis.

* @param localization Localization information of the vehicle.

* @param chassis Chassis information of the vehicle.

*/

Status Update ( const localization::LocalizationEstimate & localization,

const canbus::Chassis & chassis); //定位数据的更新，底盘的状态数据更新（CAN）

/**

* @brief Update VehicleStateProvider instance by protobuf files.

* @param localization_file the localization protobuf file.

* @param chassis_file The chassis protobuf file

*/

void Update ( const std::string & localization_file, //定位文件，底盘文件更新。

const std::string & chassis_file);

double timestamp () const ;

const localization::Pose & pose () const ; //姿态，原始姿态数据。

const localization::Pose & original_pose () const ;

/**

* @brief Default destructor.

*/

virtual ~VehicleStateProvider () = default ;

/**

* @brief Get the x-coordinate of vehicle position.

* @return The x-coordinate of vehicle position.

*/

double x () const ;

/**

* @brief Get the y-coordinate of vehicle position.

* @return The y-coordinate of vehicle position.

*/

double y () const ;

/**

* @brief Get the z coordinate of vehicle position.

* @return The z coordinate of vehicle position.

*/

double z () const ;

double kappa () const ;

/**

* @brief Get the vehicle roll angle.

* @return The euler roll angle.

*/

double roll () const ; //姿态四元素的定义：roll pitch yaw z

/**

* @brief Get the vehicle pitch angle.

* @return The euler pitch angle.

*/

double pitch () const ;

/**

* @brief Get the vehicle yaw angle.

* As of now, use the heading instead of yaw angle.

* Heading angle with East as zero, yaw angle has North as zero

* @return The euler yaw angle.

*/

double yaw () const ;

/**

* @brief Get the heading of vehicle position, which is the angle

* between the vehicle's heading direction and the x-axis.

* @return The angle between the vehicle's heading direction

* and the x-axis.

*/

double heading () const ; //汽车头部位置。

/**

* @brief Get the vehicle's linear velocity.

* @return The vehicle's linear velocity.

*/

double linear_velocity () const ;

/**

* @brief Get the vehicle's angular velocity.

* @return The vehicle's angular velocity.

*/

double angular_velocity () const ;

/**

* @brief Get the vehicle's linear acceleration.

* @return The vehicle's linear acceleration.

*/

double linear_acceleration () const ;

/**

* @brief Get the vehicle's gear position.

* @return The vehicle's gear position.

*/

double gear () const ;

/**

* @brief Set the vehicle's linear velocity.

* @param linear_velocity The value to set the vehicle's linear velocity.

*/

void set_linear_velocity ( const double linear_velocity);

/**

* @brief Estimate future position from current position and heading,

* along a period of time, by constant linear velocity,

* linear acceleration, angular velocity.

* @param t The length of time period.

* @return The estimated future position in time t.

*/

math::Vec2d EstimateFuturePosition ( const double t) const ; //限定时间内的位置估计

/**

* @brief Compute the position of center of mass(COM) of the vehicle,

* given the distance from rear wheels to the center of mass.

* @param rear_to_com_distance Distance from rear wheels to

* the vehicle's center of mass.

* @return The position of the vehicle's center of mass.

*/

math::Vec2d ComputeCOMPosition ( const double rear_to_com_distance) const ;

const VehicleState & vehicle_state () const ;

private:

bool ConstructExceptLinearVelocity (

const localization::LocalizationEstimate & localization);

common::VehicleState vehicle_state_;

localization::LocalizationEstimate original_localization_;

DECLARE_SINGLETON (VehicleStateProvider);

};

} // namespace common

} // namespace apollo

#endif // MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_PROVIDER_H_

这个头文件给出了汽车状态的各个接口，为自动控制的反馈环节提供了数据支撑。