【22日目】OSS連携してみたい(17)！LQRで経路追従(4)

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

# 前回までのあらすじ
OSS連携を試みるため、C++roboticsのLQRのコードをとりあえずビルドできるところまでやりました。
今回はAutoware連携を見越して暫定で組み込んでレーシングカーを動かしてみたいと思います。

# 今回やること
Autoware-microで提供されているpure pursuitを参考にコードを作成します。
思想としては。
初期化(?)関数でパブリッシャー、サブスクリプション等の登録を行う。
30msの定期タイマーでLRQの制御を行う。
です。

# ソースコードの構成
## Autowareのカスタマイズ
・cpprobotics_types.h
・cpprobotics.h
・cubic_spline.h
・motion_model.h
を追加します。こちらはc++roboticsからそのまま追加しました。
・my_cpp_lqr.cpp
・my_cpp_lqr.hpp
を追加します。こちらはc++roboticsのソースコードを改変したものです。
## ソースコード
```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 <motion_utils/motion_utils.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

#include <tf2/utils.h>

#include <algorithm>
#include <memory>
#include <vector>

#include "cubic_spline.h"
#include "motion_model.h"
#include "cpprobotics_types.h"
#include "my_cpp_lqr.hpp"

#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>;

namespace cpprobotics
{
    // ビルドエラー回避のため引数変更
    // 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;
    };

    State state(-0.0, -0.0, 0.0, 0.0);
    void closed_loop_prediction(Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f speed_profile)
    {
        // ビルドエラー回避
        // float stop_speed = 0.05;

        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;

        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;
    };

    MyCppLqr::MyCppLqr() : Node("my_pure_pursuit")
    {

        pub_cmd_ = create_publisher<AckermannControlCommand>("/control/command/control_cmd", 1);

        sub_kinematics_ = create_subscription<Odometry>(
            "/localization/kinematic_state", 1, [this](const Odometry::SharedPtr msg)
            { odometry_ = msg; });
        sub_trajectory_ = create_subscription<Trajectory>(
            "/planning/scenario_planning/trajectory", 1, [this](const Trajectory::SharedPtr msg)
            { trajectory_ = msg; });

        using namespace std::literals::chrono_literals;
        timer_ =
            rclcpp::create_timer(this, get_clock(), 30ms, std::bind(&MyCppLqr::onTimer, this));
    }

    AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp)
    {
        AckermannControlCommand cmd;
        cmd.stamp = stamp;
        cmd.longitudinal.stamp = stamp;
        cmd.longitudinal.speed = 0.0;
        cmd.longitudinal.acceleration = 0.0;
        cmd.lateral.stamp = stamp;
        cmd.lateral.steering_tire_angle = 0.0;
        return cmd;
    }

    bool MyCppLqr::subscribeMessageAvailable()
    {
        if (!odometry_)
        {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available");
            return false;
        }
        if (!trajectory_)
        {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available");
            return false;
        }
        return true;
    }

    void MyCppLqr::onTimer()
    {
        RCLCPP_INFO(this->get_logger(), "start onTimer()");
        // check data
        if (!subscribeMessageAvailable())
        {
            RCLCPP_INFO(this->get_logger(), "subscribeMessageAvailable not found");
            return;
        }

        // publish zero command
        AckermannControlCommand cmd = zeroAckermannControlCommand(get_clock()->now());

        Vec_f wx;
        Vec_f wy;

        for (long unsigned int i = 0; i < trajectory_->points.size(); ++i)
        {
            wx.push_back(trajectory_->points[i].pose.position.x);
            wy.push_back(trajectory_->points[i].pose.position.y);
        }

        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, odometry_->twist.twist.linear.x);
        state.x = odometry_->pose.pose.position.x;
        state.y = odometry_->pose.pose.position.y;
        state.yaw = tf2::getYaw(odometry_->pose.pose.orientation);
        state.v = odometry_->twist.twist.linear.x;
        closed_loop_prediction(r_x, r_y, ryaw, rcurvature, speed_profile);

        cmd.longitudinal.speed = state.x;
        cmd.longitudinal.acceleration = state.x;
        cmd.lateral.steering_tire_angle = state.yaw;

        RCLCPP_INFO(this->get_logger(), "Sending Cmd");
        pub_cmd_->publish(cmd);
    }
} // my_cpp_lqr

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<cpprobotics::MyCppLqr>());
    rclcpp::shutdown();
    return 0;
}

```

```my_cpp_lqr.hpp
#ifndef MY_PURE_PURSUIT_HPP_
#define MY_PURE_PURSUIT_HPP_

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <optional>
#include <rclcpp/rclcpp.hpp>

namespace cpprobotics
{

    using autoware_auto_control_msgs::msg::AckermannControlCommand;
    using autoware_auto_planning_msgs::msg::Trajectory;
    using autoware_auto_planning_msgs::msg::TrajectoryPoint;
    using geometry_msgs::msg::Pose;
    using geometry_msgs::msg::PoseWithCovarianceStamped;
    using geometry_msgs::msg::Twist;
    using nav_msgs::msg::Odometry;

    class MyCppLqr : public rclcpp::Node
    {
    public:
        explicit MyCppLqr();

        // subscribers
        rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
        rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
        rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_start_;

        // publishers
        rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_cmd_;

        // timer
        rclcpp::TimerBase::SharedPtr timer_;

        // updated by subscribers
        Trajectory::SharedPtr trajectory_;
        Odometry::SharedPtr odometry_;
        geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr
            start_;

    private:
        void onTimer();
        bool subscribeMessageAvailable();
    };
} // namespace cpprobotics

#endif // MY_PURE_PURSUIT_HPP_

```

# 実行
実行します。
左に思いっきり曲がり衝突してしまいました。
まだ改善が必要なようです。

# 今後の予定
次回以降も修正を加えながらコードの意味を理解してLQRでも動くことの確認を順にしていきたいと思います。