Apollo2.0自动驾驶之apollo/modules/common/vehicle_state/vehicle_state_provider_test.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 <string>

#include "Eigen/Core"
#include "gtest/gtest.h"

#include "modules/canbus/proto/chassis.pb.h"
#include "modules/common/util/file.h"
#include "modules/localization/common/localization_gflags.h"
#include "modules/localization/proto/localization.pb.h"

namespace apollo {
namespace common {
namespace vehicle_state_provider {

using apollo::localization::LocalizationEstimate;
using apollo::canbus::Chassis;

class VehicleStateProviderTest : public ::testing::Test {
 public:
  virtual void SetUp() {
    std::string localization_file =
        "modules/localization/testdata/3_localization_result_1.pb.txt";
    CHECK(common::util::GetProtoFromFile(localization_file, &localization_));
    chassis_.set_speed_mps(3.0);
    chassis_.set_gear_location(canbus::Chassis::GEAR_DRIVE);
    FLAGS_enable_map_reference_unify = false;
  }

 protected:
  LocalizationEstimate localization_;
  Chassis chassis_;
};

TEST_F(VehicleStateProviderTest, Accessors) {
  auto* vehicle_state_provider = VehicleStateProvider::instance();
  vehicle_state_provider->Update(localization_, chassis_);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->x(), 357.51331791372041);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->y(), 96.165912376788725);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->heading(), -1.8388082455104939);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->roll(), 0.047026695713820919);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->pitch(), -0.010712737572581465);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->yaw(), 2.8735807348741953);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->linear_velocity(), 3.0);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->angular_velocity(),
                   -0.0079623083093763921);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->linear_acceleration(),
                   -0.079383290718229638);
  EXPECT_DOUBLE_EQ(vehicle_state_provider->gear(), canbus::Chassis::GEAR_DRIVE);
}

TEST_F(VehicleStateProviderTest, EstimateFuturePosition) {
  auto* vehicle_state_provider = VehicleStateProvider::instance();
  vehicle_state_provider->Update(localization_, chassis_);
  common::math::Vec2d future_position =
      vehicle_state_provider->EstimateFuturePosition(1.0);
  EXPECT_NEAR(future_position.x(), 356.707, 1e-3);
  EXPECT_NEAR(future_position.y(), 93.276, 1e-3);
  future_position = vehicle_state_provider->EstimateFuturePosition(2.0);
  EXPECT_NEAR(future_position.x(), 355.879, 1e-3);
  EXPECT_NEAR(future_position.y(), 90.393, 1e-3);
}

}  // namespace vehicle_state_provider
}  // 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.

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

//这个文件非常有意思，在我们介绍了车辆状态的头文件，.CPP文件后，这个文件就是来测试车辆的状态信息是如何调用的。

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

#include <string>

#include "Eigen/Core"

#include "gtest/gtest.h"

#include "modules/canbus/proto/chassis.pb.h"

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

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

#include "modules/localization/proto/localization.pb.h"

namespace apollo {

namespace common {

namespace vehicle_state_provider { //车辆位置状态提供

using apollo::localization::LocalizationEstimate;

using apollo::canbus::Chassis;

class VehicleStateProviderTest : public ::testing::Test { //车辆位置状态测试

public:

virtual void SetUp () {

std::string localization_file = //这里首先加载位置状态信息，文件在如下的文件中

"modules/localization/testdata/3_localization_result_1.pb.txt" ;

CHECK ( common::util::GetProtoFromFile (localization_file, & localization_));

chassis_. set_speed_mps ( 3.0 ); //设置底盘速度，这里是每秒多少度？

chassis_. set_gear_location (canbus::Chassis::GEAR_DRIVE); //设置齿轮驱动，底盘的齿轮数据

// 要用CAN来通信，把底盘的齿轮数据传上来。

FLAGS_enable_map_reference_unify = false ;

}

protected:

LocalizationEstimate localization_;

Chassis chassis_;

};

TEST_F (VehicleStateProviderTest, Accessors) { //测试开始

auto * vehicle_state_provider = VehicleStateProvider::instance ();

vehicle_state_provider-> Update (localization_, chassis_); //更新位置、底盘数据

EXPECT_DOUBLE_EQ (vehicle_state_provider-> x (), 357.51331791372041 ); //X Y 坐标，这里的坐标
//是以车身中心来算的，请看工程中相关资料

EXPECT_DOUBLE_EQ (vehicle_state_provider-> y (), 96.165912376788725 );

EXPECT_DOUBLE_EQ (vehicle_state_provider-> heading (), - 1.8388082455104939 ); //车辆的头部的偏角，这个
//是与X的夹角

EXPECT_DOUBLE_EQ (vehicle_state_provider-> roll (), 0.047026695713820919 ); //姿态四元素

EXPECT_DOUBLE_EQ (vehicle_state_provider-> pitch (), - 0.010712737572581465 );

EXPECT_DOUBLE_EQ (vehicle_state_provider-> yaw (), 2.8735807348741953 );

EXPECT_DOUBLE_EQ (vehicle_state_provider-> linear_velocity (), 3.0 ); //线速度

EXPECT_DOUBLE_EQ (vehicle_state_provider-> angular_velocity (), //角速度

- 0.0079623083093763921 );

EXPECT_DOUBLE_EQ (vehicle_state_provider-> linear_acceleration (), //线加速度

- 0.079383290718229638 );

EXPECT_DOUBLE_EQ (vehicle_state_provider-> gear (), canbus::Chassis::GEAR_DRIVE);
//齿轮驱动状态

}

TEST_F (VehicleStateProviderTest, EstimateFuturePosition) {

auto * vehicle_state_provider = VehicleStateProvider::instance ();

vehicle_state_provider-> Update (localization_, chassis_);

common::math::Vec2d future_position =

vehicle_state_provider-> EstimateFuturePosition ( 1.0 );

EXPECT_NEAR (future_position. x (), 356.707 , 1e-3 );

EXPECT_NEAR (future_position. y (), 93.276 , 1e-3 );

future_position = vehicle_state_provider-> EstimateFuturePosition ( 2.0 );

EXPECT_NEAR (future_position. x (), 355.879 , 1e-3 );

EXPECT_NEAR (future_position. y (), 90.393 , 1e-3 );

}

} // namespace vehicle_state_provider

} // namespace common

} // namespace apollo