シミュレータと実車の差分を整理する

# はじめに

自動運転AIチャレンジ2024決勝まで、残りわずかとなりました。

https://www.jsae.or.jp/jaaic/2024ver/summary/

実車ならではの課題に取り組んでいるものの、実車に触れる機会が多くないため、
うまいことシミュレーションを活用して開発したいなと思いました。

しかし、シミュレータと実車には、少なからず乖離があります。
実際、我々のチームは、机上シミュレーションでは走破できていたものの、
実車の練習日では1周もできませんでした…😭

どこが乖離しているのか理解してシミュレータでの開発ができればいいな〜と思い、
シミュレータと実車の差分を整理してみました。

> 決勝では、コースで競技車両であるEVレーシングカートを使用した大会を行います。シミュレーションで得た知見を実車両に活かしつつ、AWSIMでは再現できない実車ならではの課題にもチャレンジします。
> 例えば参加者には実車両に適用するため、パラメータ調整にも挑戦してもらいます。また、シミュレーションでは再現できないノイズ処理、遅延対策のアルゴリズム開発も行います。

# 図解

図で整理します。まずは、制御対象がシミュレータか実車か、という当たり前な図です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/f611cf03-8ae9-a83a-9077-297470e3351d.png)

インターフェースは以下を参照ください。

https://automotiveaichallenge.github.io/aichallenge-documentation-2024/specifications/interface.html

各ブロックを見ていきます、まずはAutowareはこんな感じと思います。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/90dcb50f-55aa-c31d-e25d-7be61eef4391.png)

シミュレータは恐らくこんな感じでしょうか。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/8bd9eabc-478e-c5f2-c5dd-253660e55678.png)

実車は恐らくこんな感じでしょうか。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/5221b2dd-0a4f-2f78-de09-45786ee84a41.png)

`control_cmd`(Autoware出力) -> 各種センサ値(Autoware入力)で考えるならこんな感じでしょうか。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/cc63eddf-9ae3-056f-6e99-b6a71b9b37e1.png)

支配的なのは、実車特有のアクチュエータ遅延なのか、物理演算と現実の乖離なのか、センサ再現の乖離なのか…把握したいと思いました。


# 差分確認チャレンジ

:::note info
苦肉の策です。弊チームは実車走行データが計測できておらず、制御アルゴを固定したシミュレータと実車の比較ができません…
::: 

上図より、実車の`control_cmd`をAWSIMに流せれば、いい具合にシミュレータと実車の差分が確認できるかなあ…と思いましたのでやってみました。

運営様より共有された貴重なbagファイルがあります（弊チームは私がミスって事前練習時にbagファイルが取れていないので大変助かります…😭）。

詳細情報は以下です。

```
Files:             0920_demo_interface_only_0.db3
Bag size:          21.8 MiB
Storage id:        sqlite3
Duration:          753.149s
Start:             Sep 20 2024 17:53:37.141 (1726822417.141)
End:               Sep 20 2024 18:06:10.291 (1726823170.291)
Messages:          191602
Topic information: Topic: /control/command/actuation_cmd | Type: tier4_vehicle_msgs/msg/ActuationCommandStamped | Count: 25100 | Serialization Format: cdr
                   Topic: /control/command/control_cmd | Type: autoware_auto_control_msgs/msg/AckermannControlCommand | Count: 25106 | Serialization Format: cdr
                   Topic: /sensing/gnss/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 13972 | Serialization Format: cdr
                   Topic: /sensing/imu/imu_raw | Type: sensor_msgs/msg/Imu | Count: 13972 | Serialization Format: cdr
                   Topic: /vehicle/status/actuation_status | Type: tier4_vehicle_msgs/msg/ActuationStatusStamped | Count: 13972 | Serialization Format: cdr
                   Topic: /vehicle/status/control_mode | Type: autoware_auto_vehicle_msgs/msg/ControlModeReport | Count: 15061 | Serialization Format: cdr
                   Topic: /vehicle/status/gear_status | Type: autoware_auto_vehicle_msgs/msg/GearReport | Count: 13972 | Serialization Format: cdr
                   Topic: /vehicle/status/steering_status | Type: autoware_auto_vehicle_msgs/msg/SteeringReport | Count: 56475 | Serialization Format: cdr
                   Topic: /vehicle/status/velocity_status | Type: autoware_auto_vehicle_msgs/msg/VelocityReport | Count: 13972 | Serialization Format: cdr
```

<details><summary>全トピックはこちら</summary>

```topiclist.txt
 '/control/command/actuation_cmd/actuation/accel_cmd',
 '/control/command/actuation_cmd/actuation/brake_cmd',
 '/control/command/actuation_cmd/actuation/steer_cmd',
 '/control/command/actuation_cmd/header/stamp/nanosec',
 '/control/command/actuation_cmd/header/stamp/sec',
 '/control/command/control_cmd/lateral/stamp/nanosec',
 '/control/command/control_cmd/lateral/stamp/sec',
 '/control/command/control_cmd/lateral/steering_tire_angle',
 '/control/command/control_cmd/lateral/steering_tire_rotation_rate',
 '/control/command/control_cmd/longitudinal/acceleration',
 '/control/command/control_cmd/longitudinal/jerk',
 '/control/command/control_cmd/longitudinal/speed',
 '/control/command/control_cmd/longitudinal/stamp/nanosec',
 '/control/command/control_cmd/longitudinal/stamp/sec',
 '/control/command/control_cmd/stamp/nanosec',
 '/control/command/control_cmd/stamp/sec',
 '/sensing/gnss/pose_with_covariance/header/stamp/nanosec',
 '/sensing/gnss/pose_with_covariance/header/stamp/sec',
 '/sensing/gnss/pose_with_covariance/pose/covariance[0]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[10]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[11]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[12]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[13]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[14]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[15]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[16]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[17]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[18]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[19]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[1]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[20]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[21]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[22]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[23]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[24]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[25]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[26]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[27]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[28]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[29]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[2]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[30]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[31]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[32]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[33]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[34]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[35]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[3]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[4]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[5]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[6]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[7]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[8]',
 '/sensing/gnss/pose_with_covariance/pose/covariance[9]',
 '/sensing/gnss/pose_with_covariance/pose/pose/orientation/w',
 '/sensing/gnss/pose_with_covariance/pose/pose/orientation/x',
 '/sensing/gnss/pose_with_covariance/pose/pose/orientation/y',
 '/sensing/gnss/pose_with_covariance/pose/pose/orientation/z',
 '/sensing/gnss/pose_with_covariance/pose/pose/position/x',
 '/sensing/gnss/pose_with_covariance/pose/pose/position/y',
 '/sensing/gnss/pose_with_covariance/pose/pose/position/z',
 '/sensing/imu/imu_raw/angular_velocity/x',
 '/sensing/imu/imu_raw/angular_velocity/y',
 '/sensing/imu/imu_raw/angular_velocity/z',
 '/sensing/imu/imu_raw/angular_velocity_covariance/[0;0]',
 '/sensing/imu/imu_raw/angular_velocity_covariance/[0;1]',
 '/sensing/imu/imu_raw/angular_velocity_covariance/[0;2]',
 '/sensing/imu/imu_raw/angular_velocity_covariance/[1;1]',
 '/sensing/imu/imu_raw/angular_velocity_covariance/[1;2]',
 '/sensing/imu/imu_raw/angular_velocity_covariance/[2;2]',
 '/sensing/imu/imu_raw/header/header/stamp',
 '/sensing/imu/imu_raw/linear_acceleration/x',
 '/sensing/imu/imu_raw/linear_acceleration/y',
 '/sensing/imu/imu_raw/linear_acceleration/z',
 '/sensing/imu/imu_raw/linear_acceleration_covariance/[0;0]',
 '/sensing/imu/imu_raw/linear_acceleration_covariance/[0;1]',
 '/sensing/imu/imu_raw/linear_acceleration_covariance/[0;2]',
 '/sensing/imu/imu_raw/linear_acceleration_covariance/[1;1]',
 '/sensing/imu/imu_raw/linear_acceleration_covariance/[1;2]',
 '/sensing/imu/imu_raw/linear_acceleration_covariance/[2;2]',
 '/sensing/imu/imu_raw/orientation/pitch',
 '/sensing/imu/imu_raw/orientation/roll',
 '/sensing/imu/imu_raw/orientation/w',
 '/sensing/imu/imu_raw/orientation/x',
 '/sensing/imu/imu_raw/orientation/y',
 '/sensing/imu/imu_raw/orientation/yaw',
 '/sensing/imu/imu_raw/orientation/z',
 '/sensing/imu/imu_raw/orientation_covariance/[0;0]',
 '/sensing/imu/imu_raw/orientation_covariance/[0;1]',
 '/sensing/imu/imu_raw/orientation_covariance/[0;2]',
 '/sensing/imu/imu_raw/orientation_covariance/[1;1]',
 '/sensing/imu/imu_raw/orientation_covariance/[1;2]',
 '/sensing/imu/imu_raw/orientation_covariance/[2;2]',
 '/vehicle/status/actuation_status/header/stamp/nanosec',
 '/vehicle/status/actuation_status/header/stamp/sec',
 '/vehicle/status/actuation_status/status/accel_status',
 '/vehicle/status/actuation_status/status/brake_status',
 '/vehicle/status/actuation_status/status/steer_status',
 '/vehicle/status/control_mode/mode',
 '/vehicle/status/control_mode/stamp/nanosec',
 '/vehicle/status/control_mode/stamp/sec',
 '/vehicle/status/gear_status/report',
 '/vehicle/status/gear_status/stamp/nanosec',
 '/vehicle/status/gear_status/stamp/sec',
 '/vehicle/status/steering_status/stamp/nanosec',
 '/vehicle/status/steering_status/stamp/sec',
 '/vehicle/status/steering_status/steering_tire_angle',
 '/vehicle/status/velocity_status/header/stamp/nanosec',
 '/vehicle/status/velocity_status/header/stamp/sec',
 '/vehicle/status/velocity_status/heading_rate',
 '/vehicle/status/velocity_status/lateral_velocity',
 '/vehicle/status/velocity_status/longitudinal_velocity'
```

</details>

早速、実車`control_cmd`をシミュレータに流してみます。

まずはシミュレータの起動です。
起動後、右上の`Control`を`Manual` -> `Autonomous`にしておきます。

```
./AWSIM.x86_64
```

bagファイルの初期位置は以下で、シミュレータの初期位置と違います。
キーボード操作でシミュレータの初期位置を合わせます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/38011e14-180d-0154-9ade-1288167aee66.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/4c23e3de-279c-e489-1e1f-c8f97f123277.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/4263a1e9-f5dc-6f19-f031-3c83f06a58c2.png)

次にbagファイルとシミュレータの仲介役（`raw_vehicle_cmd_converter` / `actuation_cmd_converter` / `sensor_converter`）を起動します。

<details><summary>reference.launch.xml</summary>

```reference.launch.xml
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <!-- Essential parameters -->
  <arg name="vehicle_id" default="$(env VEHICLE_ID default)" description="vehicle specific ID"/>
  <arg name="simulation" description="used for sensor kit param"/>
  <arg name="use_sim_time"/>
  <arg name="map_path" default="$(find-pkg-share aichallenge_submit_launch)/map"/>
  <arg name="vehicle_model" default="racing_kart"  description="vehicle model name"/>
  <arg name="sensor_model" default="racing_kart_sensor_kit" description="sensor model name"/>
  <!-- Optional parameters -->
  <!-- Map -->
  <arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>

  <!-- Vehicle -->
  <arg name="launch_vehicle_interface" default="false"/>
  <log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>

  <!-- Global parameters -->
  <group scoped="false">
    <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
      <arg name="use_sim_time" value="$(var use_sim_time)"/>
      <arg name="vehicle_model" value="$(var vehicle_model)"/>
    </include>
  </group>

  <group>
    <arg name="model_file" default="$(find-pkg-share tier4_vehicle_launch)/urdf/vehicle.xacro" description="path to the file of model settings (*.xacro)"/>
    <arg name="config_dir" default="$(find-pkg-share racing_kart_sensor_kit_description)/config"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
      <param name="robot_description" value="$(command 'xacro $(var model_file) vehicle_model:=$(var vehicle_model) sensor_model:=$(var sensor_model) config_dir:=$(var config_dir) simulation:=$(var simulation)' 'warn')"/>
    </node>
  </group>

  <!-- Sensing -->
  <group>
    <push-ros-namespace namespace="sensing"/>
    <!-- Vehicle Velocity Converter  -->
    <include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml">
      <arg name="input_vehicle_velocity_topic" value="/vehicle/status/velocity_status"/>
      <arg name="output_twist_with_covariance" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
      <arg name="config_file" value="$(find-pkg-share aichallenge_submit_launch)/config/vehicle_velocity_converter.param.yaml"/>
    </include>
    <group>
      <push-ros-namespace namespace="imu"/>
      <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
      <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
        <arg name="input_topic" value="imu_raw"/>
        <arg name="output_topic" value="imu_data"/>
        <arg name="param_file" value="$(var imu_corrector_param_file)"/>
      </include>
    </group>
  </group>

    <!-- Localization -->
  <let name="pose_additional_delay_var" value="0.5" />
  <group>
    <push-ros-namespace namespace="localization"/>
    <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
      <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
      <arg name="input_imu_topic" value="/sensing/imu/imu_data"/>
      <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
      <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
    </include>

    <node pkg="imu_gnss_poser" exec="imu_gnss_poser_node" name="imu_gnss_poser" output="screen"/>

    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
      <arg name="enable_yaw_bias_estimation" value="false"/>
      <arg name="tf_rate" value="50.0"/>
      <arg name="twist_smoothing_steps" value="1"/>
      <arg name="pose_smoothing_steps" value="1"/>
      <arg name="input_initial_pose_name" value="/localization/initial_pose3d"/>
      <arg name="input_pose_with_cov_name" value="/localization/imu_gnss_poser/pose_with_covariance"/>
      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
      <arg name="output_odom_name" value="kinematic_state"/>
      <arg name="output_pose_name" value="pose"/>
      <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
      <arg name="output_biased_pose_name" value="biased_pose"/>
      <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
      <arg name="output_twist_name" value="twist"/>
      <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
      <arg name="proc_stddev_vx_c" value="10.0"/>
      <arg name="proc_stddev_wz_c" value="5.0"/>
      <arg name="pose_additional_delay" value="$(var pose_additional_delay_var)"/>
      <arg name="extend_state_step" value="100"/>
    </include>

    <!-- twist2accel -->
    <group>
      <node pkg="twist2accel" exec="twist2accel" name="twist2accel" output="screen">
        <param name="accel_lowpass_gain" value="0.9"/>
        <param name="use_odom" value="true"/>
        <remap from="input/odom" to="/localization/kinematic_state"/>
        <remap from="input/twist" to="/localization/twist_estimator/twist_with_covariance"/>
        <remap from="output/accel" to="/localization/acceleration"/>
      </node>
    </group>

  </group>

  <!-- Dummy Perception -->
  <node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
    <remap from="~/output/objects" to="/perception/object_recognition/objects"/>
  </node>

  <!-- Planning -->
  <group>
    <push-ros-namespace namespace="planning"/>

    <!-- mission_planning -->
    <group>
      <push-ros-namespace namespace="mission_planning"/>

      <!-- mission_planner -->
      <node pkg="mission_planner" exec="mission_planner" name="mission_planner" output="screen">
        <remap from="input/modified_goal" to="/planning/scenario_planning/modified_goal"/>
        <remap from="input/vector_map" to="/map/vector_map"/>
        <!-- <remap from="/localization/kinematic_state" to="/awsim/ground_truth/localization/kinematic_state"/> -->
        <remap from="debug/route_marker" to="/planning/mission_planning/route_marker"/>
        <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
      </node>

      <!-- goal_pose_visualizer -->
      <node pkg="mission_planner" exec="goal_pose_visualizer" name="goal_pose_visualizer" output="screen">
        <remap from="input/route" to="/planning/mission_planning/route"/>
        <remap from="output/goal_pose" to="/planning/mission_planning/echo_back_goal_pose"/>
      </node>

    </group> <!-- mission_planning -->

    <!-- scenario_planning -->
    <group>
      <push-ros-namespace namespace="scenario_planning"/>

      <!-- scenario_selector -->
      <group>
        <arg name="cmd" default="ros2 topic pub /planning/scenario_planning/scenario tier4_planning_msgs/msg/Scenario '{current_scenario: LaneDriving, activating_scenarios: [LaneDriving]}'"/>
        <executable cmd="$(var cmd)" name="scenario_pub" shell="true"/>
      </group> <!-- scenario_selector -->

      <!-- operation_mode -->
      <group>
        <arg name="cmd" default="ros2 topic pub /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState '{
          mode: 1,
          is_autoware_control_enabled: true,
          is_in_transition: false,
          is_stop_mode_available: true,
          is_autonomous_mode_available: true,
          is_local_mode_available: true,
          is_remote_mode_available: true
          }'"/>
        <executable cmd="$(var cmd)" name="operation_mode_pub" shell="true"/>
      </group> <!-- operation_mode -->

      <!-- lane_driving -->
      <group>
        <push-ros-namespace namespace="lane_driving"/>

        <!-- behavior_planning -->
        <group>
          <push-ros-namespace namespace="behavior_planning"/>

          <!-- behavior_planning_container -->
          <node_container pkg="rclcpp_components" exec="component_container" name="behavior_planning_container" namespace="">

            <!-- behavior_path_planner::BehaviorPathPlannerNode -->
            <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
              <remap from="~/input/route" to="/planning/mission_planning/route" />
              <remap from="~/input/vector_map" to="/map/vector_map" />
              <remap from="~/input/perception" to="/perception/object_recognition/objects" /> <!-- autoware_auto_perception_msgs/PredictedObjects -->
              <remap from="~/input/occupancy_grid_map" to="/perception/occupancy_grid_map/map" />
              <remap from="~/input/costmap" to="/planning/scenario_planning/parking/costmap_generator/occupancy_grid" />
              <remap from="~/input/odometry" to="/localization/kinematic_state" />
              <remap from="~/input/accel" to="/localization/acceleration" />
              <remap from="~/input/scenario" to="/planning/scenario_planning/scenario" />
              <remap from="~/output/path" to="path_with_lane_id" />
              <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd" />
              <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd" />
              <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal" />
              <param name="bt_tree_config_path" value="$(find-pkg-share aichallenge_submit_launch)/config/behavior_path_planner_tree.xml"/>
              <param name="lane_change.enable_abort_lane_change" value="false"/>
              <param name="lane_change.enable_collision_check_at_prepare_phase" value="false"/>
              <param name="lane_change.use_predicted_path_outside_lanelet" value="false"/>
              <param name="lane_change.use_all_predicted_path" value="false"/>
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/common/nearest_search.param.yaml" />
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml" />
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml" />
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml" />
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml" />
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml" />
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/behavior_path_planner.param.yaml" />
              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml" />
              <extra_arg name="use_intra_process_comms" value="false" />
            </composable_node>

          </node_container>
        </group> <!-- behavior_planning -->
      </group> <!-- lane_driving -->

      <!-- Customizable -->
      <node pkg="path_to_trajectory" exec="path_to_trajectory_node" name="path_to_trajectory" output="screen">
        <remap from="input" to="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
        <remap from="output" to="/planning/scenario_planning/trajectory"/>
      </node>

    </group>
  </group>

  <!-- Pure Pursuit -->
  <let name="steering_tire_angle_gain_var" value="1.0" if="$(var simulation)"/>
  <let name="steering_tire_angle_gain_var" value="1.639" unless="$(var simulation)"/>

  <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node" output="screen">
    <param name="use_external_target_vel" value="true"/>
    <param name="external_target_vel" value="4.16667"/>
    <param name="lookahead_gain" value="0.5"/>
    <param name="lookahead_min_distance" value="3.5"/>
    <param name="speed_proportional_gain" value="1.0"/>
    <param name="steering_tire_angle_gain" value="$(var steering_tire_angle_gain_var)"/>

    <remap from="input/kinematics" to="/localization/kinematic_state"/>
    <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
    <remap from="output/control_cmd" to="/control/command/control_cmd_sim"/>
  </node>

  <!-- Map -->
  <group>
    <push-ros-namespace namespace="map"/>

    <!-- map_container -->
    <node_container pkg="rclcpp_components" exec="component_container" name="map_container" namespace="">

      <!-- map_loader::Lanelet2MapLoaderNode -->
      <composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader" namespace="">
        <remap from="output/lanelet2_map" to="vector_map" />
        <param name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
        <param from="$(find-pkg-share aichallenge_submit_launch)/config/map/lanelet2_map_loader.param.yaml" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

      <!-- map_loader::Lanelet2MapVisualizationNode -->
      <composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="lanelet2_map_visualization" namespace="">
        <remap from="input/lanelet2_map" to="vector_map" />
        <remap from="output/lanelet2_map_marker" to="vector_map_marker" />
        <param name="lanelet2_map_path" value="$(var map_path)/$(var lanelet2_map_file)" />
        <param from="$(find-pkg-share aichallenge_submit_launch)/config/map/lanelet2_map_loader.param.yaml" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

      <!-- map_tf_generator::VectorMapTFGeneratorNode -->
      <composable_node pkg="map_tf_generator" plugin="VectorMapTFGeneratorNode" name="vector_map_tf_generator" namespace="">
        <param name="map_frame" value="map" />
        <param name="viewer_frame" value="viewer" />
        <extra_arg name="use_intra_process_comms" value="false" />
      </composable_node>

    </node_container>

  </group> <!-- map -->

  <!-- vehicle -->
  <group if="$(var launch_vehicle_interface)">
    <include file="$(find-pkg-share raw_vehicle_cmd_converter)/launch/raw_vehicle_converter.launch.xml">
      <arg name="converter_param_path" value="$(find-pkg-share aichallenge_submit_launch)/config/converter.param.yaml"/>
      <arg name="csv_path_accel_map" value="$(find-pkg-share aichallenge_submit_launch)/data/accel_map.csv"/>
      <arg name="csv_path_brake_map" value="$(find-pkg-share aichallenge_submit_launch)/data/brake_map.csv"/>

      <arg name="max_throttle" value="1.0"/>
      <arg name="max_brake" value="1.0"/>
      <arg name="convert_accel_cmd" value="true"/>
      <arg name="convert_brake_cmd" value="true"/>
      <arg name="convert_steer_cmd" value="false"/>

      <arg name="input_control_cmd" value="/control/command/control_cmd"/>
      <arg name="input_odometry" value="/localization/kinematic_state"/>
      <arg name="input_steering" value="/vehicle/status/steering_status"/>
      <arg name="output_actuation_cmd" value="/control/command/actuation_cmd"/>
    </include>
  </group>

  <!-- API -->
  <group>
    <!-- default_ad_api -->
    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py" />

    <!-- ad_api_adaptors -->
    <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" />
  </group>

</launch>
```

</details>

```
ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=true use_sim_time:=false run_rviz:=true
```


最後にbagファイルを再生します。加速開始の直前までオフセットしておきます。

```
ros2 bag play 0920_demo_interface_only_0.db3 --clock --start-offset 418 --topics /control/command/control_cmd
```


ノード図はこんな感じです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/ba6d8414-9c17-55f8-f141-d72ad4c76bb4.png)


一応、車両が動くことは確認できたのですが、なかなかうまく走れませんでした。
本来1周可能な`control_cmd`なのですが、何度初期位置を調整しても、ステアリングを切りすぎて壁にぶつかってしまいます。

![ezgif-1-81e76580bf.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/2b66873c-8230-bc19-e34a-390b5d4008c2.gif)

(追記)後述しますが、実車走行時はControlCmdにゲイン1.639をかけていることが原因かも。逆補正が必要と思われる。

# 実車データ分析

想定していた差分確認は叶いませんでした。
せっかくなので、実車データそのものを分析して、実車の特性を確認してみます。

## 実車特有のアクチュエータ遅延

横方向、縦方向に分けて見てみます。

### 横方向

以下の流れで、指令値に対して実舵が変わります。

`/control/command/control_cmd`
　-> `/control/command/actuation_cmd`
　-> `/vehicle/status/actuation_status`
　-> `/vehicle/status/steering_status`


![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/159d202c-63bb-7e4d-70c0-19cbbfdb7fef.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/84fdbd9d-31f7-ddcf-a81a-35f26b23df24.png)

まず`control_cmd/steering_tire_angle`と`actuation_cmd/steer_cmd`は一致します（＝マップを使った変換はしていないです）。


`control_cmd/steering_tire_angle` -> `vehicle/status/steering_tire_angle`は、
0.2秒くらいの遅延はありそうですがほぼほぼ無視できるレベルだと思います。

※ 既にシミュレータに反映されているそうです。

https://github.com/AutomotiveAIChallenge/aichallenge-2024/pull/143

実車特有のアクチュエータ遅延などはなさそうです。

### 縦方向

以下の流れで、指令値に対して加速度が変わります。

`/control/command/control_cmd`
　-> `/control/command/actuation_cmd`
　-> `/vehicle/status/actuation_status`
　-> `/vehicle/status/velocity_status`

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/31931147-cb0d-c36d-7b9b-cd8144eb7231.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/50cde102-4073-518f-27b2-c8b63bd7cb96.png)

こちらは少しややこしく、`control_cmd/acceleration`と`actuation_cmd/accel_cmd`は一致せず、accel_cmdを0~1に収まるようにマップを使って変換しています。

本来は、`control_cmd/acceleration` -> `vehicle/status/acceleration`を確認したいのですが、後者が存在せず、代替信号もなさそうだった（`/sensing/imu/imu_raw/linear_acceleration` や `/vehicle/status/velocity_status/longitudinal_velocity`はノイズまみれ、平滑化してもイマイチだった）ので、`accel_cmd` -> `accel_status`を見てみます。

0.2秒くらいの遅延はありそうですがほぼほぼ無視できるレベルだと思います。

こちらも、実車特有のアクチュエータ遅延などはなさそうです。

## 物理演算と現実の乖離

次はこちらを確認してみます。

### ホイールベースの同定

色々項目はあると思いますが、今回は二輪モデルを仮定して、ヨーレイト $\gamma$ と車速 $v$ 、タイヤステアリング角 $\delta$ から ホイールベース $l$ を同定してみます。

https://myenigma.hatenablog.com/entry/20140301/1393648106

使う信号はこちら。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/c9e85e0a-fb44-d238-68d7-6569cfcfa523.png)

計算式はこちら。

```math
\displaylines{
l = \frac{v}{\gamma} * \sin\delta
}
```

結果が以下です。
公称スペックは`1.087`ですが、`1.28`程度のほうが当てはまりが良さそうです。


![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/b14417e7-1e9d-9849-5828-ef57dd34e90d.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/64a5938d-2d32-4a7b-3c2d-3af834b417d2.png)

https://automotiveaichallenge.github.io/aichallenge-documentation-2024/specifications/simulator.html

ちなみにシミュレーションだとこうなります。
こちらは`1.087`のほうが当てはまりが良さそうです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/5af2d7ca-a0fe-d4bf-9981-720f3cfd427d.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/353dc567-958e-705b-6dd2-1b84a424d636.png)

ヨーレイトは以下の式で生成されます。

```math
\displaylines{
\gamma = \frac{v}{l} * \sin\delta
}
```

分母のホイールベースが大きく同定されたということは、ヨーレイトが発生しにくい傾向を表しています。つまり、実車は **曲がりにくい** ことを表しているのではないでしょうか。


## センサ再現

確認できていないです…　基本的には実車を模した設定がなされているはずです。

https://github.com/AutomotiveAIChallenge/aichallenge-2024/pull/118

# シミュレータの差分を吸収するには

完璧なシミュレータなんて存在しないと思いますので、
こちらの検討をするほうが有益だと思います。

## ステアリング指令値の補正

実車は曲がりにくいので、ゲインをかけて補正する方法が今大会は採用されています。

https://github.com/AutomotiveAIChallenge/aichallenge-2024/blob/main/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml#L212

## 実データを使ったデータドリブンなチューニング

実データを使ってチューニングする方法があるそうです。
今回は適用できませんでしたが、とても面白そうです。

https://qiita.com/WandererEng/items/27dae0d4ef0b1195024d

## モデル誤差抑制補償器

この辺も興味あります。
「あれ、シミュレータと違うな？」をFBするようなイメージでも使えるのでしょうか。

https://www.jstage.jst.go.jp/article/sicejl/62/3/62_168/_pdf/-char/ja

# おわりに

実車はシミュレータより曲がりにくい、という特性が見えてきました。

他にも実車固有の特性はありそうですが、今回はここまで…

この記事は、AI文章校正ツール「ちゅらいと」で校正されています。

(余談) 難しいですよね、シミュレータって…　
というか本来シミュレータは現実と乖離がある前提のものかなと思っています。一発目に実機で試して高価な機材を破壊してしまうことを避けるために、まずは最低限実機テストに耐えうるレベルを机上で作ろうよがシミュレータの存在意義・モチベーションだと思います。そういった意味では機能していると思います。