【20日目】OSS連携してみたい(16)！LQRで経路追従(3)

この記事は筆者オンリーの[Advent Calendar 2023](https://qiita.com/advent-calendar/2023/aichallenge_racing_for_mas)20日目の記事です。  

# 前回までのあらすじ
OSS連携を試みるため、C++でROS2の簡単なモジュールを作りました。
今回はLQRのソースコードをAutowareに組み込んでみます。
pure pursuitではPythonRoboticsを使用しましたが、今回はC++roboticsを使ってみます。
[こちら](https://github.com/onlytailei/CppRobotics/blob/master/src/lqr_speed_steer_control.cpp)のソースコードがビルドできるところまでちょっと進めてみたいと思います。
とりあえず暫定でビルドできるとこまでの確認です。

# ROS2でモジュール作成
[ROSのチュートリアル](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html)を参考に雛形を作成します。

# Autowareのカスタマイズ
## パッケージの作成
Autowareをカスタマイズするために、ROSのモジュールを追加します。

```
$ cd ~/aichallenge2023-racing/docker/aichallenge/aichallenge_ws/src/aichallenge_submit/
$ ros2 pkg create --build-type ament_cmake my_cpp_lqr
```

[ROSチュートリアル](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html)に従って、package.xmlとCMakeLists.txtを変更します。

## パッケージのビルド
次に、Dockerを起動してコードをビルドします。
C++RoboticsのコードからOpenCVをサポートしていないこと、未使用変数を使っている場合エラーとなることを考慮して暫定でプログラムを作成しました。
```my_cpp_lqr.cpp
/*************************************************************************
    > File Name: lqr_steer_control.cpp
    > Author: TAI Lei
    > Mail: ltai@ust.hk
    > Created Time: Wed Apr 17 11:48:46 2019
 ************************************************************************/

#include <iostream>
#include <limits>
#include <vector>
#include <sys/time.h>
#include <Eigen/Eigen>
#include "cubic_spline.h"
#include "motion_model.h"
#include "cpprobotics_types.h"

#define DT 0.1
#define L 0.5
#define KP 1.0
#define MAX_STEER 45.0 / 180 * M_PI

using namespace cpprobotics;
using Matrix5f = Eigen::Matrix<float, 5, 5>;
using Matrix52f = Eigen::Matrix<float, 5, 2>;
using Matrix25f = Eigen::Matrix<float, 2, 5>;
using RowVector5f = Eigen::Matrix<float, 1, 5>;
using Vector5f = Eigen::Matrix<float, 5, 1>;

// ビルドエラー回避のため引数変更
// Vec_f calc_speed_profile(Vec_f rx, Vec_f ry, Vec_f ryaw, float target_speed)
Vec_f calc_speed_profile(Vec_f ryaw, float target_speed)
{
    Vec_f speed_profile(ryaw.size(), target_speed);

    float direction = 1.0;
    for (unsigned int i = 0; i < ryaw.size() - 1; i++)
    {
        float dyaw = std::abs(ryaw[i + 1] - ryaw[i]);
        float switch_point = (M_PI / 4.0 < dyaw) && (dyaw < M_PI / 2.0);

        if (switch_point)
            direction = direction * -1;
        if (direction != 1.0)
            speed_profile[i] = target_speed * -1;
        else
            speed_profile[i] = target_speed;

        if (switch_point)
            speed_profile[i] = 0.0;
    }

    for (int k = 0; k < 40; k++)
    {
        *(speed_profile.end() - k) = target_speed / (50 - k);
        if (*(speed_profile.end() - k) <= 1.0 / 3.6)
        {
            *(speed_profile.end() - k) = 1.0 / 3.6;
        }
    }
    return speed_profile;
};

float calc_nearest_index(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, int &ind)
{
    float mind = std::numeric_limits<float>::max();
    for (unsigned int i = 0; i < cx.size(); i++)
    {
        float idx = cx[i] - state.x;
        float idy = cy[i] - state.y;
        float d_e = idx * idx + idy * idy;

        if (d_e < mind)
        {
            mind = d_e;
            ind = i;
        }
    }
    float dxl = cx[ind] - state.x;
    float dyl = cy[ind] - state.y;
    float angle = YAW_P2P(cyaw[ind] - std::atan2(dyl, dxl));
    if (angle < 0)
        mind = mind * -1;

    return mind;
};

Matrix5f solve_DARE(Matrix5f A, Matrix52f B, Matrix5f Q, Eigen::Matrix2f R)
{
    Matrix5f X = Q;
    int maxiter = 150;
    float eps = 0.01;

    for (int i = 0; i < maxiter; i++)
    {
        Matrix5f Xn = A.transpose() * X * A - A.transpose() * X * B * (R + B.transpose() * X * B).inverse() * B.transpose() * X * A + Q;
        Matrix5f error = Xn - X;
        if (error.cwiseAbs().maxCoeff() < eps)
        {
            return Xn;
        }
        X = Xn;
    }

    return X;
};

Matrix25f dlqr(Matrix5f A, Matrix52f B, Matrix5f Q, Eigen::Matrix2f R)
{
    Matrix5f X = solve_DARE(A, B, Q, R);
    Matrix25f K = (B.transpose() * X * B + R).inverse() * (B.transpose() * X * A);
    return K;
};

Vec_f lqr_steering_control(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f sp, float &pe, float &pth_e)
{
    int ind = 0;
    float e = calc_nearest_index(state, cx, cy, cyaw, ind);

    float k = ck[ind];
    float th_e = YAW_P2P(state.yaw - cyaw[ind]);
    float tv = sp[ind];

    Matrix5f A = Matrix5f::Zero();
    A(0, 0) = 1.0;
    A(0, 1) = DT;
    A(1, 2) = state.v;
    A(2, 2) = 1.0;
    A(2, 3) = DT;
    A(4, 4) = 1.0;

    Matrix52f B = Matrix52f::Zero();
    B(3, 0) = state.v / L;
    B(4, 1) = DT;

    Matrix5f Q = Matrix5f::Identity();
    Eigen::Matrix2f R = Eigen::Matrix2f::Identity();

    // gain of lqr
    Matrix25f K = dlqr(A, B, Q, R);

    Vector5f x = Vector5f::Zero();
    x(0) = e;
    x(1) = (e - pe) / DT;
    x(2) = th_e;
    x(3) = (th_e - pth_e) / DT;
    x(4) = state.v - tv;

    Eigen::Vector2f ustar = -K * x;

    float ff = std::atan2((L * k), (double)1.0);
    float fb = YAW_P2P(ustar(0));
    float delta = ff + fb;
    float ai = ustar(1);

    pe = e;
    pth_e = th_e;
    return {ai, delta};
};

void update(State &state, float a, float delta)
{

    if (delta >= MAX_STEER)
        delta = MAX_STEER;
    if (delta <= -MAX_STEER)
        delta = -MAX_STEER;

    state.x = state.x + state.v * std::cos(state.yaw) * DT;
    state.y = state.y + state.v * std::sin(state.yaw) * DT;
    state.yaw = state.yaw + state.v / L * std::tan(delta) * DT;
    state.v = state.v + a * DT;
};

void closed_loop_prediction(Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f speed_profile, Poi_f goal)
{
    float T = 500.0;
    float goal_dis = 0.3;
    // ビルドエラー回避
    // float stop_speed = 0.05;

    State state(-0.0, -0.0, 0.0, 0.0);

    float time_ = 0.0;
    Vec_f x;
    x.push_back(state.x);
    Vec_f y;
    y.push_back(state.y);
    Vec_f yaw;
    yaw.push_back(state.yaw);
    Vec_f v;
    v.push_back(state.v);
    Vec_f t;
    t.push_back(0.0);

    float e = 0;
    float e_th = 0;

    // ビルドエラー回避
    // int count = 0;
    Vec_f x_h;
    Vec_f y_h;

    while (T >= time_)
    {
        Vec_f control = lqr_steering_control(state, cx, cy, cyaw, ck, speed_profile, e, e_th);
        // float ai = KP * (speed_profile[ind]-state.v);
        update(state, control[0], control[1]);
        // if (std::abs(state.v) <= stop_speed) ind += 1;

        float dx = state.x - goal[0];
        float dy = state.y - goal[1];
        if (std::sqrt(dx * dx + dy * dy) <= goal_dis)
        {
            std::cout << ("Goal") << std::endl;
            break;
        }

        x_h.push_back(state.x);
        y_h.push_back(state.y);

        // save image in build/bin/pngs
        struct timeval tp;
        gettimeofday(&tp, NULL);
        long int ms = tp.tv_sec * 1000 + tp.tv_usec / 1000;
        std::string int_count = std::to_string(ms);
    }
};

int main()
{
    Vec_f wx({0.0, 6.0, 12.5, 10.0, 17.5, 20.0, 25.0});
    Vec_f wy({0.0, -3.0, -5.0, 6.5, 3.0, 0.0, 0.0});

    Spline2D csp_obj(wx, wy);
    Vec_f r_x;
    Vec_f r_y;
    Vec_f ryaw;
    Vec_f rcurvature;
    Vec_f rs;
    for (float i = 0; i < csp_obj.s.back(); i += 0.1)
    {
        std::array<float, 2> point_ = csp_obj.calc_postion(i);
        r_x.push_back(point_[0]);
        r_y.push_back(point_[1]);
        ryaw.push_back(csp_obj.calc_yaw(i));
        rcurvature.push_back(csp_obj.calc_curvature(i));
        rs.push_back(i);
    }
    float target_speed = 10.0 / 3.6;
    // ビルドエラー回避
    // Vec_f speed_profile = calc_speed_profile(r_x, r_y, ryaw, target_speed);
    Vec_f speed_profile = calc_speed_profile(ryaw, target_speed);
    closed_loop_prediction(r_x, r_y, ryaw, rcurvature, speed_profile, {{wx.back(), wy.back()}});
}

```


# 今後の予定
C++Roboticsのコードがビルドが通るところまでは確認しました。
しかしながらROSにまだ対応していないこと、Autowareにまだ対応していないことから修正がまだまだ必要です。
次回以降は修正を加えながらコードの意味を理解してLQRでも動くことの確認を順にしていきたいと思います。