Apollo2.0自動駕駛之apollo/modules/common/vehicle_state/vehicle_state_provider_test.cc

/****************Apollo源碼分析****************************

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

Contact with: [email protected] 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