ロボットの仕様とセンサの取り付け

ロボット自身の形は 2 つに分かれていて、それぞれ別のコンポーネントで与えます。

センサの取り付けには標準のコンポーネントを使います。RobotSpecManager はセンサの取り付けを出しません。標準にない、輪郭、寸法・質量、性能値（速度・加速度の限界）を出します。

2 つのファイルは 1 つにまとめられません。Sensor Calibration File Parser は、知らない要素（たとえば <footprint>）があるファイルを読めないので、ロボットの仕様と、センサの校正は別のファイルにしてください。

座標

• ロボットの座標は ROS の REP-103 に従います：右手系、X が前、Y が左、Z が上、単位は m。原点はロボットの姿勢（pose2D）が指す点です。
• 取り付けの向きは roll・pitch・yaw で、R = Rz(yaw)·Ry(pitch)·Rx(roll) の順です（Rotation3D、ICP 系の lidar_mount_* と同じ）。

RobotSpecManager：ロボットの輪郭

RobotSpecManager はロボットの仕様ファイルを読み、輪郭と半径をカスタムストラクチャー SMSL_RobotSpec（出力 robot_spec）で、始めに 1 回だけ出します。輪郭を使う部品（NavigationToolkit の膨張・計画・安全停止・表示など）は線で受けるので、ロボットの大きさをそれぞれのプロパティに持ちません。

プロパティは spec_file（ロボットの仕様ファイルのパス）だけです。

<robot_spec>
  <!-- rectangle / circle / polygon のどれか 1 つ -->
  <footprint shape="rectangle" front_in_m="0.3" rear_in_m="0.3" left_in_m="0.2" right_in_m="0.2"/>
  <!-- <footprint shape="circle" radius_in_m="0.25"/> -->
  <!-- <footprint shape="polygon">0.3 0.2  -0.3 0.2  -0.3 -0.2  0.3 -0.2</footprint> -->
</robot_spec>

• 出す輪郭は必ず反時計回りです。時計回りで与えた多角形は並びを逆にします。
• 内接円の半径は原点から辺までの最短距離、外接円の半径は原点から頂点までの最長距離です。円の場合は、どちらも radius_in_m です（輪郭は円を内に含む正 16 角形）。
• ファイルが読めないとき、多角形が自分と交わるか面積が 0 のとき、原点が輪郭の内側にないときは、エラーで止まります。構造体は DataViewer で見えないので、値をログに出します（Robot spec: vertices = ..., inscribed_radius_in_m = ..., circumscribed_radius_in_m = ...）。
• ロボットのまわりの余裕（ROS の footprint_padding）は、ロボットの仕様に入れません。輪郭を使う部品がそれぞれ持ちます。

種類、本体、性能値

ロボットの仕様ファイルには、輪郭のほかに、ロボットの物理値と性能値も書けます。後段の部品のパラメータを減らすためです。ロボットの種類を <robot_spec> の type に書きます（<body> か <motion_limits> を書くときは必須）：differential（差動二輪・クローラ・AGV）、omni（メカナム・オムニホイール）、ackermann（前輪操舵）、drone（マルチロータ）。

• <body>（任意）：本体の高さと下端の高さ（height_in_m、ground_clearance_in_m）、質量と重心（mass_in_kg、cog_xyz_in_m）、車輪まわり（wheel_radius_in_m、track_width_in_m、wheelbase_in_m、drive_axle_x_in_m、max_steering_angle_in_rad、max_drive_force_in_N）。輪郭と同じ構造体 SMSL_RobotSpec（出力 robot_spec）で出ます。地上のロボットは、ロボット座標の z = 0 が接地面です。
• <motion_limits>（任意）：機体の限界。最大速度（前進・後退・横・上昇・下降）、最大角速度、最大加速度と減速度（安全停止の停止距離を決める非常停止の減速度を含む）、ドローンでは傾きの最大角、ackermann では舵角を切る速さの最大（max_steering_rate_in_rad_per_s。旋回の角速度とその変化は、速度・舵角・ホイールベースで決まるので書きません：ω = v·tan(δ)/L、dω/dt = (a·tan δ + v·δ̇/cos²δ)/L）。構造体 SMSL_RobotMotionLimits（出力 robot_motion_limits）で出ます。この出力は <motion_limits> を書いたときだけできます。書くのは機体の限界だけです。今日使う速さ（「ゆっくり」）は、使う部品のプロパティに、限界以下の値で持ちます。
• 必須・任意・書かない属性は、種類によって違います。種類に合わない値（たとえば差動二輪の wheelbase_in_m）を書くと、エラーで止まります。ロボットが動けない方向は、出力が 0 になります。
• 摩擦係数はここにありません。ロボットではなく床の値だからです。

<robot_spec type="differential">
  <footprint shape="rectangle" front_in_m="0.3" rear_in_m="0.3" left_in_m="0.2" right_in_m="0.2"/>
  <body height_in_m="0.8" mass_in_kg="40" cog_xyz_in_m="0.05 0 0.25"
        wheel_radius_in_m="0.1" track_width_in_m="0.4" drive_axle_x_in_m="0"/>
  <motion_limits max_velocity_forward_in_m_per_s="1.0" max_velocity_backward_in_m_per_s="0.3"
          max_yaw_rate_in_rad_per_s="1.5"
          max_acceleration_in_m_per_s2="0.5" max_deceleration_in_m_per_s2="1.0"
          max_yaw_acceleration_in_rad_per_s2="3.0"/>
</robot_spec>

4 つの種類のサンプルは、example フォルダの robot_spec_differential.xml、robot_spec_omni.xml、robot_spec_ackermann.xml、robot_spec_drone.xml です。種類ごとにどの属性が要るかは、ファイルのコメントに書いてあります。

Sensor Calibration File Parser：センサの取り付け

Sensor Calibration File Parser は、センサの座標系どうしの変換を書いた XML を読み、4 × 4 の変換行列を出します。直接書いていない 2 つのセンサの間も、ほかの座標系を通ってつながっていれば（たとえば LiDAR からロボットを通ってカメラへ）変換を求めます。逆向きの変換も求めます。

ファイル

<extrinsic from="A" to="B"> は、A の座標系の点を B の座標系へ移す変換です。中に書いた変換は、書いた順に適用されます。

<?xml version="1.0" encoding="UTF-8" ?>
<system>
  <calibration>
    <extrinsic from="front_lidar" to="robot">
      <!-- X 軸まわりに roll 20 度、次に Y 軸まわりに pitch −30 度、次に Z 軸まわりに yaw 40 度 -->
      <rotation><axis_angle><vector size="3">1 0 0</vector><angle unit="deg">20</angle></axis_angle></rotation>
      <rotation><axis_angle><vector size="3">0 1 0</vector><angle unit="deg">-30</angle></axis_angle></rotation>
      <rotation><axis_angle><vector size="3">0 0 1</vector><angle unit="deg">40</angle></axis_angle></rotation>
      <!-- ロボット座標でのセンサの位置（m） -->
      <translation><vector size="3">0.25 0 0.3</vector></translation>
    </extrinsic>
  </calibration>
</system>

• パーサーには roll・pitch・yaw の書き方がありません。X・Y・Z 軸まわりの 3 つの回転をこの順に書き、そのあとに平行移動を書きます。結果は p_robot = Rz(yaw)·Ry(pitch)·Rx(roll)·p_sensor + t で、Rotation3D の式と同じです（RTMaps で Rotation3D の Matrix Input につないで確かめました）。
• 回転は、3 × 3 の行列、ロドリゲスのベクトル、クォータニオンでも書けます。4 × 4 の行列 [R | t] は <transform> に書けます。
• センサの名前（from / to）は自由な文字です。ファイルとコンポーネントのプロパティで同じ名前を使います。

コンポーネント

点群に当てる

出力 matrix_<i> を Rotation3D の入力 transformation_matrix_4x4 につなぎ、transfo_source = Matrix Input (4x4) にします。点群での Rotation3D の設定は、PointCloudProcessingToolkit のトピック「点群の座標：センサからロボットへ」にあります。

例：GNSS のアンテナと IMU のレバーアーム

IMU を内蔵しない GNSS では、アンテナと IMU は別の場所にあります。この 2 つのずれがレバーアーム l です。それぞれの位置をロボットに対して書くと、2 つの間の変換はパーサーがロボットを通って求めます。

<?xml version="1.0" encoding="UTF-8" ?>
<system>
  <calibration>
    <extrinsic from="gnss_antenna" to="robot">
      <translation><vector size="3">0.5 0 1.2</vector></translation>
    </extrinsic>
    <extrinsic from="imu" to="robot">
      <translation><vector size="3">0.1 0 0.3</vector></translation>
    </extrinsic>
  </calibration>
</system>

matrix_0_from = gnss_antenna、matrix_0_to = imu にします。出力の行列 [R | t] の平行移動 t が、IMU の座標で見たアンテナの位置、つまり l です。この例では l = (0.4, 0, 0.9) m です。

• from と to を入れ替えると、逆向きの変換になります。この例の平行移動は (-0.4, 0, -0.9) m です。
• l は IMU の軸で表した値です。IMU を回して付けたときは、ファイルにその <rotation> を足します。IMU を Z 軸まわりに 90 度回して付けると、l = (0, -0.4, 0.9) m です。

これらの値は、RTMaps で Rotation3D の Matrix Input につないで確かめました。

パーサーが出すのは、この固定の l だけです。GNSS の位置を補正するには、そのときどきの姿勢も要ります。p_antenna = p_imu + R(t)·l で（R(t) は IMU の姿勢）、速度には ω × l の項が入ります。この行列は姿勢で変わらないので、Rotation3D では補正になりません。補正は、位置を推定する部品の中で行います。

imu_to_pose6D：IMU の姿勢

IMU の姿勢のクォータニオンを pose6D [0, 0, 0, roll, pitch, yaw] にします。機種ごとの違いは 2 つのプロパティで受けます。

NED の入力を ENU のまま読むと、ピッチの符号が逆になり、yaw も 90° − 方位になりません。お使いの機種の並びと座標は、メーカーの資料で確かめてください。

出力は IMU 自身の姿勢です。IMU をロボットに対して回して付けたときは、IMU → ロボットの回転を Sensor Calibration File Parser の校正ファイルに書き、姿勢を使う側で当てます。

IMU は位置を測れないので、並進は常に 0 です。ヨーの基準は機種によります。地磁気を使う機種は北が基準で、6 軸の機種はドリフトします。

どちらを使うか