ロボットのコードの単体テストをやってみることにした

この記事は「[大分高専 Advent Calendar 2024](https://adventar.org/calendars/10845)」6か7日目の記事です。
## はじめに
大分高専 Advent Calendar 2024 に参加させていただきました、やまです。記事を書くのは初めてなので，色々誤字脱字など見苦しいところがあるかもしれませんが，この記事が誰かの役煮立てば幸いです．

## 単体テストの必要性
部活などで日常的にロボットを制御する方ならご理解いただけると思うのですが，実機がメンテナンスや加工中などの関係で完成してない状況だと，書いたコードの試験が満足にできない...ってことあると思います．で，いざ実機試験するとバグが出まくって設計班から今まで何してたんこいつみたいな顔，されます．まあごもっともではあるので，今回は「テストコードを書いて実機なしでもデバッグしよう！」ということをやっていきます．

## 使用するツール
今回は，「Google Test」というツールを使って単体テストを行います．
```shell:google test
git submodule add https://github.com/google/googletest.git test/googletest
cd test/googletest
cmake -S . -B build
cmake --build build
cmake --install build
```
まずは，任意のリポジトリにgoogle testを入れていきます．今回は，`test`フォルダに色々書いていくつもりなので`test/googletest`にsubmoduleとして追加します．

インストールが終わったらCMakeLists.txtを記述して
```cmake:test/CMakeLists.txt
cmake_minimum_required(VERSION 3.18)

find_package(GTest REQUIRED)
include(GoogleTest)

add_executable(Test01 test.cpp)

target_link_libraries(Test01 GTest::GTest GTest::Main)

#ここからは各自のファイルなどをinclude
include_directories(lib)
include_directories(pkg)
#ここまで


enable_testing()
gtest_discover_tests(Test01)
```

あとは個人的に利用してるライブラリとかを記述したりしていきます．

## 既存コードの手直し
今回は，自分が高専三年生の時に書いた「国崩し」の自己位置推定のコードをテストしてみることにします．
```cpp:undercarriage.cpp
void Undercarriage::calc_position(void)
{
    float xplace;
    float yplace;
    float xdis;
    float ydis;
    float xplus;
    float yplus;
    float xturn;
    float yturn;

    static float before_xplace;
    static float before_yplace;
    static float before_xturn;
    static float before_yturn;

    static float before_yaw;
    static float rotation_times;

    float pulse[2];
    
    float yaw = (this->m_gyro[0]->get() + this->m_gyro[1]->get()) / 2.0;
    
    float rad = deg_to_rad(yaw);

    this->enc_getAsFloat(pulse);

    xplace = pulse[0] * DIAMETER / RESOLUTION * PI * -1.0;
    yplace = pulse[1] * DIAMETER / RESOLUTION * PI;

    xturn = CENTERDISTANCE *  rad;
    yturn = CENTERDISTANCE * -rad;

    xdis = (xplace - xturn) - (before_xplace - before_xturn);
    ydis = (yplace - yturn) - (before_yplace - before_yturn);

    xplus = (xdis * cos(rad)) - (ydis * sin(rad));
    yplus = (ydis * cos(rad)) + (xdis * sin(rad));

    before_xplace = xplace;
    before_yplace = yplace;
    before_xturn = xturn;
    before_yturn = yturn;

    before_yaw = yaw;
    
    this->m_position.x += xplus;
    this->m_position.y += yplus;
    this->m_position.yaw = yaw;

}
```
入力と出力が分かりづらいですが，入力は`m_gyro[0].get()`や`enc_getAsFloat(pulse)`を用いて行われていますね．出力は`m_position`に値を加算して行われています．
（本来なら構造体や`std::pair`や`tuple`,`map`などで，入力は引数を用いて，出力は`return`で返したほうが値が追いやすくていいと思う）


GyroクラスやEncoderクラスの設計がそのままだとoverrideとかを利用したテスト時に支障が出るのでInterfaceを利用したものに変更しておきます．
```cpp:Interfaceを用いた設計変更
class IEncoder
{
public:
    virtual float getAsFloat(void) = 0;
};


class Encoder_dep : public mechanism::IEncoder
{
private:
    can_smbus::Encoder* m_enc;
public:

    Encoder_dep(can_smbus::Encoder* enc) : m_enc(enc) {};
    float get(void)
    {
        return m_enc->getAsFloat();
    }
};
```
こうすることで，今までのコードは`Encoder_dep`を通して互換性が保たれるかつ，`IEncoder`をInterfaceとして用いたテスト用のクラスを使うことが出来るようになって都合がいいです．
あとついでに自己位置推定のクラスも内部宣言からDI(依存性注入)するものに変えておきます．僕もよく分かってないですが，オブジェクトを内部ではなく外部で宣言することでクラスの再利用性とかが高まるらしいです．日常からテストを意識したコードを書くことは大切ですね．（1敗）

## テストコード
さて，いよいよ本題のテストコードを書いていきます．
```cpp:test.cpp
#include <gtest/gtest.h>
#include <gmock/gmock.h>

//TEST_F(クラス名，テスト名)
TEST_F(TestUndercarriage, test1)
{
    
}

int main(int argc, char **argv)
{
    //テスト実行の初期化
    testing::InitGoogleTest(&argc, argv);
    //テスト返却
    return RUN_ALL_TESTS();
}
```
基本の雛形はこんな感じです．
ここでGyroとEncoderをモック（一部機構をテスト用のものに置き換えること）します．
```cpp:test.cpp
//MOCK_METHOD(返り値, 関数名, (引数), (修飾子))

class Mock_enc : public mechanism::IEncoder
{
public:
    MOCK_METHOD(float, getAsFloat, (), (override));
    //getAsFloatをモック
};

class Mock_Gyro : public mechanism::IGyro
{
public:
    MOCK_METHOD(float, get, (), (override));
    //getをモック
};
```
このとき，仮想関数でないとオーバーライド出来ません．だから，Interfaceにまとめる必要があったんですね．

続いてはテストするクラスの実装です．
```cpp:test.cpp
class TestUndercarriage : public ::testing::Test
{
public:
    Mock_enc mockEnc1;
    Mock_enc mockEnc2;
    Mock_Gyro mockGyro1;
    Mock_Gyro mockGyro2;

    mechanism::Undercarriage* undercarriage;

    TestUndercarriage() {
        undercarriage = new mechanism::Undercarriage(&mockEnc1, &mockEnc2, &mockGyro1, &mockGyro2);
    }
};
```
先程のクラスを宣言して，クラスにぶち込んでいきます．ここの`Undercarriage`も内部宣言だとテストに苦労するのでDI(依存性注入)を用いたコードにリファクタリングする必要がありました．日常からテストを意識したコードを書くことは大切ですね．（2敗）

それでは，実際にテストの中身を書いていきます．
```cpp:test.cpp
TEST_F(TestUndercarriage, test1)
{
    EXPECT_CALL(mockEnc1, getAsFloat())
        .WillRepeatedly([]() 
        {
            static float counter = 0;
            counter += 1.2;  // 呼び出されるごとに1.2増加
            return counter;
        });
        
    EXPECT_CALL(mockEnc2, getAsFloat()).WillRepeatedly(testing::Return(0.0f));
    //0.0を毎回返す

    EXPECT_CALL(mockGyro1, get()).WillRepeatedly([]()
    {
        static float counter = 0;
        counter += 0.05;
        return counter;
    });
    //呼び出されるごとに0.05増加
    
    EXPECT_CALL(mockGyro2, get()).WillRepeatedly(testing::Return(0.0f));
    //0.0を毎回返す

    mechanism::position_t pos = {.x=0, .y=0, .yaw=0};

    for(int i = 0; i < 100; i++)
    {
        undercarriage->calc_position();
        undercarriage->get_position(&pos);

        std::cout << "x: " << pos.x << " y: " << pos.y << " yaw: " << pos.yaw << std::endl;
    }
}
```
内容としては，Encoder xの値を少しずつ増しながらちょっとずつ回転していくという挙動のロボットの自己位置を出力するものとなります．本来はEXPECT_EQとかを使ってテストの可否を出力したほうがいいのですが，今回は挙動を見たかっただけということでスルーします．

というわけで，
```cpp:test.txt
#include "../lib/library.hpp"
#include "../pkg/package.hpp"
#include <gtest/gtest.h>
#include <gmock/gmock.h>

class Mock_enc : public mechanism::IEncoder
{
public:
    MOCK_METHOD(float, getAsFloat, (), (override));

};

class Mock_Gyro : public mechanism::IGyro
{
public:
    MOCK_METHOD(float, get, (), (override));
};

class TestUndercarriage : public ::testing::Test
{
public:
    Mock_enc mockEnc1;
    Mock_enc mockEnc2;
    Mock_Gyro mockGyro1;
    Mock_Gyro mockGyro2;

    mechanism::Undercarriage* undercarriage;

    TestUndercarriage() {
        undercarriage = new mechanism::Undercarriage(&mockEnc1, &mockEnc2, &mockGyro1, &mockGyro2);
    }
};

TEST_F(TestUndercarriage, test1)
{
    
    EXPECT_CALL(mockEnc1, getAsFloat())
        .WillRepeatedly([]() 
        {
            static float counter = 0;
            counter += 1.2;  // 次回呼び出しに備えて1増加
            return counter;
        });
    EXPECT_CALL(mockEnc2, getAsFloat()).WillRepeatedly(testing::Return(0.0f));
    //EXPECT_CALL(mockGyro1, get()).WillRepeatedly(testing::Return(0.0f));

    EXPECT_CALL(mockGyro1, get()).WillRepeatedly([]()
    {
        static float counter = 0;
        counter += 0.05;
        return counter;
    });
    EXPECT_CALL(mockGyro2, get()).WillRepeatedly(testing::Return(0.0f));

    mechanism::position_t pos = {.x=0, .y=0, .yaw=0};

    std::cout << "Undercarriage constructor enc1:" << &mockEnc1 << std::endl;
    for(int i = 0; i < 100; i++)
    {
        undercarriage->calc_position();
        undercarriage->get_position(&pos);

        std::cout << "x: " << pos.x << " y: " << pos.y << " yaw: " << pos.yaw << std::endl;
    }
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}
```
```shell:出力
root@5e6978f5f3c8:/workspaces/RobotRMM_2024# ./build/test/Test01
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from TestUndercarriage
[ RUN      ] TestUndercarriage.test1
x: -0.575585 y: 0.133489 yaw: 0.025
x: -1.15123 y: 0.266727 yaw: 0.05
x: -1.72693 y: 0.399714 yaw: 0.075
x: -2.30269 y: 0.532449 yaw: 0.1
〜中略〜
x: -56.0865 y: 11.7755 yaw: 2.425
x: -56.6672 y: 11.8845 yaw: 2.45
x: -57.2479 y: 11.9933 yaw: 2.475
x: -57.8287 y: 12.1018 yaw: 2.5
[       OK ] TestUndercarriage.test1 (2 ms)
[----------] 1 test from TestUndercarriage (2 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (2 ms total)
[  PASSED  ] 1 test.
```
が一連のテストコードとなります．まあ今回は簡単なアルゴリズムのテストなので実用性は感じにくいとは思いますが，モジュール単位の試験を手軽にできるというのは大きなメリットかなと思います．

## 終わりに
ちょっと手間はかかりましたが，毎年大会直前や大事な時期になってバグで沼るプログラマーは多数観測されているので，日々しっかりテストして頑張りたいですね．次回はFlutterとかROS2とかにも手を出すかもしれません．