如何在 Gazebo URDF/SDF 中加入 IMU？

🔹 (1) 在 URDF 檔案中加入 IMU

📌 在 robot_arm_urdf.urdf 內加入 IMU 定義

xml
  <link name="imu_link">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.02"/>
      </geometry>
      <material name="Gray"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.02"/>
      </geometry>
    </collision>
  </link>

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 0.1"/>
  </joint>

  <gazebo reference="imu_link">
    <sensor type="imu" name="imu_sensor">
      <update_rate>100.0</update_rate>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>100.0</updateRate>
        <bodyName>imu_link</bodyName>
        <topicName>imu/data</topicName>
        <frameName>imu_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

✅ 這樣 Gazebo 會在 base_link 上安裝 IMU，並發布 IMU 數據到 imu/data topic

---

🔹 (2) 在 SDF (robot_arm.sdf) 內加入 IMU

📌 如果使用 SDF 格式

xml
  <sensor name="imu_sensor" type="imu">
    <always_on>true</always_on>
    <update_rate>100.0</update_rate>
    <imu>
      <angular_velocity>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.002</stddev>
        </noise>
      </angular_velocity>
      <linear_acceleration>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.002</stddev>
        </noise>
      </linear_acceleration>
    </imu>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
      <topicName>imu/data</topicName>
      <frameName>imu_link</frameName>
    </plugin>
  </sensor>

✅ 這樣 Gazebo 會生成 imu/data 的 ROS topic，並且模擬加速度 & 角速度數據！

---

📌 2️⃣ 啟動 Gazebo 並測試 IMU

📌 啟動 Gazebo

bash
roslaunch robot_arm_urdf robot_arm_control.launch

✅ Gazebo 內會出現 imu_link，IMU Plugin 會開始發布數據

📌 查看 IMU Topic

bash
rostopic list | grep imu

✅ 應該看到

bash
/imu/data

📌 監聽 IMU 數據

bash
rostopic echo /imu/data

✅ 應該輸出

yaml
header:
  seq: 123
  stamp:
    secs: 10
    nsecs: 123456
  frame_id: "imu_link"
orientation:
  x: 0.001
  y: -0.002
  z: 0.003
  w: 1.000
angular_velocity:
  x: 0.01
  y: -0.01
  z: 0.02
linear_acceleration:
  x: 0.05
  y: -0.03
  z: 9.81

✅ 表示 IMU 已成功運行，並且測量出機械手臂的加速度與角速度！

---

📌 3️⃣ 使用 rviz 可視化 IMU

📌 啟動 rviz

bash
rosrun rviz rviz

📌 在 rviz 中

1. 點擊 Add
2. 選擇 IMU
3. 設定 Topic 為 /imu/data ✅ 你應該會看到 IMU 的方向與加速度變化！

---

📌 4️⃣ 進階設定

🔹 (1) 設定 IMU 測量誤差

在 IMU Plugin 內加入 noise 參數：

xml
<imu>
  <angular_velocity>
    <noise type="gaussian">
      <mean>0.0</mean>
      <stddev>0.002</stddev>
    </noise>
  </angular_velocity>
</imu>

✅ 這會讓 IMU 測量時產生隨機誤差 (Gaussian Noise)，模擬真實 IMU 數據。

---

🔹 (2) 設定更高頻率的數據更新

在 Plugin 內修改 update_rate：

xml
<updateRate>500.0</updateRate>

✅ 這樣 IMU 每秒會更新 500 次，讓資料更即時。

---

🚀 總結

步驟	指令 / 設定
1. 在 URDF / SDF 加入 IMU	使用 libgazebo_ros_imu_sensor.so 生成 /imu/data
2. 啟動 Gazebo	roslaunch robot_arm_urdf robot_arm_control.launch
3. 監聽 IMU 數據	rostopic echo /imu/data
4. Rviz 內可視化 IMU	在 Rviz Add -> IMU -> /imu/data
5. 設定 IMU 誤差	加入 noise 參數
6. 提高更新頻率	設定 updateRate 為 500.0

🚀 這樣你可以在 Gazebo 內模擬 IMU 並與 ROS 進行整合！ 🚀