自動運転AIチャレンジでローカルプランニングを自作する 2

## 前回

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/4b514a7d-fff5-d3b4-bcc9-d16188e4c775.png" width=400>


こんなかんじのイメージの作りたい！ということで準備していました．

## 方針

Frenet Frameを用いた軌道最適化を試してみようと考えました．手法については，以下に執筆中です．

https://qiita.com/yhisaki/items/e50a1b7e0aaf92111bdf

## 実装方法

基本的に[Python Robotics](https://github.com/AtsushiSakai/PythonRobotics)を参考に実装します．

障害物を避ける前のtrajectoryは`/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory` というTopicで，メッセージの型は以下のようになっているっぽいです．

<details>
<summary>Trajectory</summary>

```Trajectory.idl
#include "autoware_auto_planning_msgs/msg/TrajectoryPoint.idl"
#include "std_msgs/msg/Header.idl"

module autoware_auto_planning_msgs {
  module msg {
    module Trajectory_Constants {
      const uint32 CAPACITY = 10000;
    };
    @verbatim (language="comment", text=
      " A set of trajectory points for the controller")
    struct Trajectory {
      std_msgs::msg::Header header;

      sequence<autoware_auto_planning_msgs::msg::TrajectoryPoint, 10000> points;
    };
  };
};
```

</details>

また，pointsは，以下のようになってます．

<details>
<summary>TrajectoryPoint</summary>

```TrajectoryPoint.idl
#include "builtin_interfaces/msg/Duration.idl"

module autoware_auto_planning_msgs {
  module msg {
    @verbatim (language="comment", text=
      " Representation of a trajectory point for the controller")
    struct TrajectoryPoint {
      builtin_interfaces::msg::Duration time_from_start;

      geometry_msgs::msg::Pose pose;

      @default (value=0.0)
      float longitudinal_velocity_mps;

      @default (value=0.0)
      float lateral_velocity_mps;

      @default (value=0.0)
      float acceleration_mps2;

      @default (value=0.0)
      float heading_rate_rps;

      @default (value=0.0)
      float front_wheel_angle_rad;

      @default (value=0.0)
      float rear_wheel_angle_rad;
    };
  };
};

```

</details>


色々な情報がありますが，実際にtopicの中を見てみると，`time_from_start`はすべて0になっていて，`pose`はしっかり値が入っていたのですが，`longitudinal_velocity_mps`は1.38で一定値，あとは0になっていました．

障害物に関しては，`/perception/object_recognition/objects`というTopicにあり，メッセージ型は，

<details>
<summary>PredictedObjects</summary>

```PredictedObjects.idl
#include "autoware_auto_perception_msgs/msg/PredictedObject.idl"
#include "std_msgs/msg/Header.idl"

module autoware_auto_perception_msgs {
  module msg {
    @verbatim (language="comment", text=
      " This is the output of object prediction.")
    struct PredictedObjects {
      std_msgs::msg::Header header;
      sequence<autoware_auto_perception_msgs::msg::PredictedObject> objects;
    };
  };
};

```

</details>

となっており，`object`はPredictedObjectという型で，

<details>
<summary>PredictedObject</summary>

```PredictedObject.idl
#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl"
#include "autoware_auto_perception_msgs/msg/Shape.idl"
#include "autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl"
#include "unique_identifier_msgs/msg/UUID.idl"

module autoware_auto_perception_msgs {
  module msg {
    struct PredictedObject {
      unique_identifier_msgs::msg::UUID object_id;

      @range (min=0.0, max=1.0)
      float existence_probability;

      sequence<autoware_auto_perception_msgs::msg::ObjectClassification> classification;
      autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics;

      autoware_auto_perception_msgs::msg::Shape shape;
    };
  };
};
```

</details>

となっています．障害物の位置は`kinematics`にあり，

<details>
<summary>PredictedObjectKinematics</summary>

```PredictedObjectKinematics.idl
#include "autoware_auto_perception_msgs/msg/PredictedPath.idl"
#include "geometry_msgs/msg/AccelWithCovariance.idl"
#include "geometry_msgs/msg/PoseWithCovariance.idl"
#include "geometry_msgs/msg/TwistWithCovariance.idl"

module autoware_auto_perception_msgs {
  module msg {
    struct PredictedObjectKinematics {
      geometry_msgs::msg::PoseWithCovariance initial_pose_with_covariance;

      geometry_msgs::msg::TwistWithCovariance initial_twist_with_covariance;

      geometry_msgs::msg::AccelWithCovariance initial_acceleration_with_covariance;

      sequence<autoware_auto_perception_msgs::msg::PredictedPath, 100> predicted_paths;
    };
  };
};
```

</details>

の中の`initial_pose_with_covariance`にありました．

ある適当な時刻で，これらのデータをとってきてプロットすると以下のようになります．

![Figure_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/4a035069-46a2-5beb-0dc6-92fb6927e990.png)

実装のイメージはいかのような感じです．

1. 障害物を避ける前のtrajectory(`refernce_path`)をsplineで補完する．
1. 以下のような関数を実装し，障害物を避けたtrajectoryを計算する．
    ```python3
    optimized_path = frenet_optimization(refernce_path_spline, obstacle_position, obstacle_radius)
    ```
    

## Frenet Frameの動作確認

![Figure_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/cd80f003-0e42-65d9-7789-a003d146e98b.png)

障害物の付近である程度軌道を歪められています．
ただ，pythonで実装しているせいか，計算に22秒も時間がかかってしまいます．もう少し，パラメータや実装を調整してai-challengeに組み込んでみたいと思います！