diff --git a/docs/course/control.md b/docs/course/control.md deleted file mode 100644 index 5dea2cc..0000000 --- a/docs/course/control.md +++ /dev/null @@ -1 +0,0 @@ -# 04. 制御 \ No newline at end of file diff --git a/docs/course/images/2-3/PlotJuggler1.png b/docs/course/images/2-3/PlotJuggler1.png new file mode 100644 index 0000000..f0a039c Binary files /dev/null and b/docs/course/images/2-3/PlotJuggler1.png differ diff --git a/docs/course/images/2-3/PlotJuggler2.png b/docs/course/images/2-3/PlotJuggler2.png new file mode 100644 index 0000000..113132e Binary files /dev/null and b/docs/course/images/2-3/PlotJuggler2.png differ diff --git a/docs/course/images/2-3/PlotJuggler3.png b/docs/course/images/2-3/PlotJuggler3.png new file mode 100644 index 0000000..a2ebb35 Binary files /dev/null and b/docs/course/images/2-3/PlotJuggler3.png differ diff --git a/docs/course/images/2-3/PlotJuggler4.png b/docs/course/images/2-3/PlotJuggler4.png new file mode 100644 index 0000000..981f6ed Binary files /dev/null and b/docs/course/images/2-3/PlotJuggler4.png differ diff --git a/docs/course/images/2-3/PlotJuggler5.png b/docs/course/images/2-3/PlotJuggler5.png new file mode 100644 index 0000000..3c08cde Binary files /dev/null and b/docs/course/images/2-3/PlotJuggler5.png differ diff --git a/docs/course/images/2-3/PlotJuggler6.png b/docs/course/images/2-3/PlotJuggler6.png new file mode 100644 index 0000000..29c92d7 Binary files /dev/null and b/docs/course/images/2-3/PlotJuggler6.png differ diff --git a/docs/course/images/2-3/node_diagram.png b/docs/course/images/2-3/node_diagram.png new file mode 100644 index 0000000..aba7527 Binary files /dev/null and b/docs/course/images/2-3/node_diagram.png differ diff --git a/docs/course/images/2-3/velocity_planning_image.jpg b/docs/course/images/2-3/velocity_planning_image.jpg new file mode 100644 index 0000000..a3a2dea Binary files /dev/null and b/docs/course/images/2-3/velocity_planning_image.jpg differ diff --git a/docs/course/images/localization_node.png b/docs/course/images/localization_node.png new file mode 100644 index 0000000..2bcb374 Binary files /dev/null and b/docs/course/images/localization_node.png differ diff --git a/docs/course/images/p_controller_plotjuggler.png b/docs/course/images/p_controller_plotjuggler.png new file mode 100644 index 0000000..0ea438d Binary files /dev/null and b/docs/course/images/p_controller_plotjuggler.png differ diff --git a/docs/course/images/plotjuggler_1.png b/docs/course/images/plotjuggler_1.png new file mode 100644 index 0000000..34eee9d Binary files /dev/null and b/docs/course/images/plotjuggler_1.png differ diff --git a/docs/course/images/plotjuggler_2.png b/docs/course/images/plotjuggler_2.png new file mode 100644 index 0000000..583b44e Binary files /dev/null and b/docs/course/images/plotjuggler_2.png differ diff --git a/docs/course/images/practice_localization_node.png b/docs/course/images/practice_localization_node.png new file mode 100644 index 0000000..95ef7a7 Binary files /dev/null and b/docs/course/images/practice_localization_node.png differ diff --git a/docs/course/localization.md b/docs/course/localization.md deleted file mode 100644 index fe5f61e..0000000 --- a/docs/course/localization.md +++ /dev/null @@ -1,5 +0,0 @@ -# 03. 自己位置推定 - -!!! warning - - 作成中 \ No newline at end of file diff --git a/docs/course/path_planning.md b/docs/course/path_planning.md new file mode 100644 index 0000000..537be2b --- /dev/null +++ b/docs/course/path_planning.md @@ -0,0 +1,19 @@ +# 03. 経路計画 + +## 03-01. 障害物に衝突しないように経路計画を行う + +!!! warning + + 作成中 + +## 03-02. 経路を基に軌道計画を行う + +!!! warning + + 作成中 + +## 03-03. PIDとpure pursuitで車両を軌道に追従させる + +!!! warning + + 作成中 diff --git a/docs/course/perception.md b/docs/course/perception.md index 8bb89c5..6fb6845 100644 --- a/docs/course/perception.md +++ b/docs/course/perception.md @@ -1,5 +1,19 @@ -# 07. 外界認識 +# 04. 外界認識 + +## 04-01. lidarから点群を取得し障害物検知をする + +!!! warning + + 作成中 + +## 04-02. 点群情報に基づいて経路・軌道計画し車両を追従させる + +!!! warning + + 作成中 + +## 04-03. PIDとpure pursuitで車両を軌道に追従させる !!! warning - 作成中 \ No newline at end of file + 作成中 diff --git a/docs/course/planning.md b/docs/course/planning.md deleted file mode 100644 index 987c373..0000000 --- a/docs/course/planning.md +++ /dev/null @@ -1,5 +0,0 @@ -# 06. 経路計画 - -!!! warning - - 作成中 \ No newline at end of file diff --git a/docs/course/rviz.md b/docs/course/rviz.md deleted file mode 100644 index c6db632..0000000 --- a/docs/course/rviz.md +++ /dev/null @@ -1,5 +0,0 @@ -# 05. 可視化ツール - -!!! warning - - 作成中 \ No newline at end of file diff --git a/docs/course/sensing.md b/docs/course/sensing.md deleted file mode 100644 index 290a0a2..0000000 --- a/docs/course/sensing.md +++ /dev/null @@ -1,13 +0,0 @@ -# 02. センサーデータの利用 - -## 02-01. IMUデータの利用 - -!!! warning - - 作成中 - -## 02-02. GNSSデータの利用 - -!!! warning - - 作成中 diff --git a/docs/course/velocity_planning.md b/docs/course/velocity_planning.md new file mode 100644 index 0000000..659ccf1 --- /dev/null +++ b/docs/course/velocity_planning.md @@ -0,0 +1,292 @@ +# 02. 速度計画 + +安全な自動運転を行うためには常に速度をコントロールする必要があります。 +ここでは速度計画を行って目標地点で車両を停止させることを目指します。 + +## 02-01. 車速を取得する + +まず、車両の速度を取得してみましょう。 + +車両の速度は/localization/kinematic_stateというトピックでパブリッシュされます。 +/localization/kinematic_stateには自車両の位置、姿勢、速度、角速度とそれらの共分散行列の情報が含まれています。 + +本来Autowareでは以下のノードダイアグラムのように、GNSS、Lidar、IMUなどの情報をもとにekf_localizerが/localization/kinematic_stateを計算します。 + +
+ Alt Text +
+ Autowareのlocalizer周りのノードダイアグラム +
+ +
+ +しかし初心者にAutowareの説明をするのにこの構成はかなり複雑なので、今回は以下のようにシンプルな +localizerを用意しました。 + +
+ +
+ Alt Text +
+ autoware-practiceのlocalizer周りのノードダイアグラム +
+ +
+ +01-01と同様に以下のコマンドでシミュレータを起動してから、 + +```bash +ros2 launch autoware_practice_launch practice.launch.xml +``` + +別のターミナルで以下のコマンドでトピックから車両の速度を取得してみましょう。 +```bash +ros2 topic echo /localization/kinematic_state +``` +車両の初期速度は0なので、以下のように`twist: linear: x: 0.0`と表示されます。 +``` +header: + stamp: + sec: 1713775224 + nanosec: 319370472 + frame_id: odom +child_frame_id: base_link +pose: + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + covariance: + - 0.1 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.1 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.1 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.01 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.01 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.01 +twist: + twist: + linear: + x: 0.0 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 0.0 + covariance: + - 0.01 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.01 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.01 +--- +``` + +[/localization/kinematic_state]()というトピックは [nav_msgs/msgs/Odometry](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Pose.html) というROS2のメッセージ型を利用しています。 + + +## 02-02. 車両速度を目標速度に収束させる + +次に、車両が目標の速度になるように車両の速度を制御してみましょう。 +現在の速度v_nowと目標の速度v_targetの差にゲインk_pをかけたものを加速度入力aとする比例制御を用いることを考えます。 + + + +$$ a = k_{\text{p}} \cdot (v_{\text{target}} - v_{\text{now}}) $$ + +以下に速度の比例制御を行うノードのサンプルを用意しました。 +- [velocity_planning/p_controller.hpp](https://github.com/AutomotiveAIChallenge/autoware-practice/blob/main/src/autoware_practice_course/src/velocity_planning/p_controller.hpp) +- [velocity_planning/p_controller.cpp](https://github.com/AutomotiveAIChallenge/autoware-practice/blob/main/src/autoware_practice_course/src/velocity_planning/p_controller.cpp) + + +以下のコマンドで比例制御のコントローラのノードを起動することができ、車両が動き始めます。kpに比例ゲイン、target_velocityに速度[m/s]を格納します。 +```bash +ros2 run autoware_practice_course p_controller --ros-args -p kp:=0.5 -p target_velocity:=1.0 +``` + +速度の時間推移をグラフで確認するために、シミュレーションデータをrosbagに保存してPlotJugglerで図示してみましょう。 + +新しいターミナルで以下のコマンドを実行しておくことで特定のトピックの時系列データをrosbag形式で保存することができます。 + +```bash +ros2 bag record -o velocity.bag /localization/kinematic_state +``` + +PlotJugglerは以下のコマンドで、インストールすることができます。 +```bash +sudo apt install ros-humble plotjuggler-ros +``` + +そして以下のコマンドでPlotJugglerを起動します。 +```bash +ros2 run plotjuggler plotjuggler +``` + +左上のDataをクリックし、先程保存したautoware-practice/velocity.bag/metadata.yamlを選択し右上のOpenをクリックします。 + +![plotjuggler_1](./images/plotjuggler_1.png) + +/localization/kinematic_stateをクリックし右下のOKをクリックします。 + +左下のTimeseries.Listからlocalization > kinematic_state > twist > twist > linear > x を選択し、右側にドラッグ＆ドロップをすると速度の時間推移を表すグラフを表示することができます。 + +![plotjuggler_2](./images/plotjuggler_2.png) + +最後に以下の比例ゲインk_pを0.5から5.0に修正して、車両速度の収束速度を比べてみましょう。 + +$$ a = k_{\text{p}} \cdot (v_{\text{target}} - v_{\text{now}}) $$ + +```bash +ros2 run autoware_practice_course p_controller --ros-args -p kp:=5.0 -p target_velocity:=1.0 +``` + +今回は比例ゲインKを大きくすることで、早く目標速度に収束することがわかります（左: k_p=0.5, 右: k_p=5.0）。 + +![p_controller_plotjuggler](./images/p_controller_plotjuggler.png) + +## 02-03. ゴールで停止するための速度計画を行う + +図のような、停止状態から50m地点まで加速し、50m地点を過ぎたら減速し100m地点で停止するような速度計画を行うことを考えます。 + +![alt text](<./images/2-3/velocity_planning_image.jpg>) + +速度計画を行うために車両から目標地点までの間に1mおきに中継地点となるウェイポイントを設定します。各ウェイポイントに目標速度を設定することで速度計画を行います。 + +各ウェイポイントの目標速度を決定するためのtrajectory_plannerノードと、各ウェイポイントを基に制御入力を決定するlongitudinal_controllerノードを作成しました。 + +![alt text](./images/2-3/node_diagram.png) + +trajectory_plannerノードがスタートからゴールまでの各ウェイポイントの目標速度を決定します。 +longitudinal_controllerノードが車両に最も近いウェイポイントを探索し、ウェイポイントでの目標速度と現在の車両の速度を基に制御入力を決定します。 + +各ノードは以下のコマンドを別々のターミナルで実行することで起動することができます。 + +```bash +ros2 launch autoware_practice_launch practice.launch.xml +``` + +```bash +ros2 run autoware_practice_course trajectory_planner +``` + +```bash +ros2 run autoware_practice_course longitudinal_controller --ros-args -p kp:=5.0 +``` + +車両の位置と速度の関係をPlotJugglerを用いてリアルタイムで確認してみましょう。 +全部で4枚のターミナルウィンドウを使用します。 +まずシミュレータを起動します。 +```bash +ros2 launch autoware_practice_launch practice.launch.xml +``` + +次に別ターミナルでPlotJugglerを起動します。 + +```bash +ros2 run plotjuggler plotjuggler +``` + +PlotJugglerが起動したらStartボタンを押します。 + +![alt text](./images/2-3/PlotJuggler1.png) + +/localization/kinematic_stateを選択し、OKを押します。 + +![alt text](./images/2-3/PlotJuggler2.png) + +/localization/kinematic_state/pose/pose/position/xと/localization/kinematic_state/twist/twist/linear/xを複数選択して**右クリック**でドラッグ＆ドロップすることで、位置と速度の関係のグラフを見ることができます。 + +![alt text](./images/2-3/PlotJuggler3.png) + +初めは車両の位置と速度がどちらも0なので以下の図のようになります。 + +![alt text](./images/2-3/PlotJuggler4.png) + +長時間記録するためにBufferを100に変更します。 + +![alt text](./images/2-3/PlotJuggler6.png) + +次にtrajectory_plannerノードとlongitudinal_controllerノードをそれぞれ別のターミナルで起動します。 + +```bash +ros2 run autoware_practice_course trajectory_planner +``` + +```bash +ros2 run autoware_practice_course longitudinal_controller --ros-args -p kp:=5.0 +``` +それぞれのノードが起動するとシミュレーター上で車両が動き始め、以下のようなグラフが得られます。 +![alt text](./images/2-3/PlotJuggler5.png) + +グラフより、大体50m付近で10m/sに達し100m付近で停止できていることがわかります。 + + \ No newline at end of file diff --git a/docs/specifications/interface.ja.md b/docs/specifications/interface.ja.md index a9ef523..2460046 100644 --- a/docs/specifications/interface.ja.md +++ b/docs/specifications/interface.ja.md @@ -1 +1,83 @@ # インターフェース + +## 一覧 + +| Interface | Name | Type | +| ------------ | ------------------------------------ | -------------------------------------------------------- | +| Subscription | `/control/command/control_cmd` | `autoware_auto_control_msgs/msg/AckermannControlCommand` | +| Publisher | `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs/msg/VelocityReport` | +| Publisher | `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs/msg/SteeringReport` | +| Subscription | `/control/command/gear_cmd` | `autoware_auto_vehicle_msgs/msg/GearCommand` | +| Publisher | `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs/msg/GearReport` | +| Publisher | `/sensing/gnss/pose_with_covariance` | `geometry_msgs/msg/PoseWithCovarianceStamped` | +| Publisher | `/sensing/imu/imu_data` | `sensor_msgs/msg/Imu` | + + + +### `/control/command/control_cmd` + +| Name | Description | +| ----------------------------------- | -------------------- | +| stamp | メッセージの送信時刻 | +| lateral.stamp | 未使用 | +| lateral.steering_tire_angle | T.B.D. | +| lateral.steering_tire_rotation_rate | T.B.D. | +| longitudinal.stamp | 未使用 | +| longitudinal.speed | T.B.D. | +| longitudinal.acceleration | T.B.D. | +| longitudinal.jerk | 未使用 | + +### `/vehicle/status/velocity_status` + +| Name | Description | +| --------------------- | ------------------------ | +| header.stamp | データの取得時刻 | +| header.frame_id | フレームID (`base_link`) | +| longitudinal_velocity | 速度 | +| lateral_velocity | T.B.D. | +| heading_rate | T.B.D | + +### `/vehicle/status/steering_status` + +| Name | Description | +| ------------------- | ---------------- | +| stamp | データの取得時刻 | +| steering_tire_angle | タイヤ角度 | + +### `/control/command/gear_cmd` + +| Name | Description | +| ------- | -------------------- | +| stamp | メッセージの送信時刻 | +| command | ギアの種類 | + +### `/vehicle/status/gear_status` + +| Name | Description | +| ------ | ---------------- | +| stamp | データの取得時刻 | +| report | ギアの種類 | + +### `/sensing/gnss/pose_with_covariance` + +| Name | Description | +| --------------------- | -------------------------------------------- | +| header.stamp | データの取得時刻 | +| header.frame_id | フレームID (`map`) | +| pose.pose.position | 車両位置 (フレームID `base_link` 原点の位置) | +| pose.pose.orientation | 未使用 | +| pose.covariance | 位置精度 | + +### `/sensing/imu/imu_data` + +| Name | Description | +| ------------------- | ------------------------ | +| header.stamp | データの取得時刻 | +| header.frame_id | フレームID (`base_link`) | +| orientation | 方位 | +| angular_velocity | 角速度 | +| linear_acceleration | 加速度 | diff --git a/mkdocs.yaml b/mkdocs.yaml index c1a7b90..34ed5d1 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -25,11 +25,8 @@ nav: - course: - course/index.md - course/vehicle.md - - course/sensing.md - - course/localization.md - - course/control.md - - course/rviz.md - - course/planning.md + - course/velocity_planning.md + - course/path_planning.md - course/perception.md - specifications: - specifications/interface.md