(Robot_arm)gazebo/model_states topic YAML 解析與用途

 

📌 YAML 解析與用途

這份 YAML 檔案的格式與 gazebo_msgs/ModelStates 非常相似，應該用於 Gazebo 內部的機器人狀態描述，包含：

• name：定義場景內的物件與機器人連結（links）。
• pose：物件的 位置（position）與方向（orientation）。
• twist：物件的 線速度（linear velocity）與角速度（angular velocity）。

---

1️⃣ YAML 結構解析

🔹 name（物件名稱）

yaml
name:
  - ground_plane::link
  - robot_arm_urdf::base_link
  - robot_arm_urdf::link_1
  - robot_arm_urdf::link_2
  - robot_arm_urdf::link_3
  - robot_arm_urdf::link_4
  - robot_arm_urdf::link_5
  - robot_arm_urdf::link_6
  - robot_arm_urdf::link_7

• 這代表 Gazebo 內場景的模型，包含：
  • ground_plane::link（地面）
  • robot_arm_urdf::base_link（機械臂基座）
  • robot_arm_urdf::link_1 ~ link_7（機械臂的七個關節）

---

🔹 pose（位姿）

yaml
pose:
  -
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

• position (x, y, z)：物體在 Gazebo 世界座標系 中的位置。
• orientation (x, y, z, w)：使用四元數（Quaternion）來表示物體的旋轉角度。
• 目前所有物件 都設為 (0,0,0) 並無旋轉。

---

🔹 twist（速度）

yaml
twist:
  -
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0

• linear (x, y, z)：物體的線速度（m/s）。
• angular (x, y, z)：物體的角速度（rad/s）。
• 目前所有物體 靜止不動。

---

2️⃣ 這份 YAML 可能的用途

1. Gazebo /gazebo/model_states 訂閱輸出的內容

   • 這個格式類似 rostopic echo /gazebo/model_states 取得的資料，可能是從 Gazebo 匯出的狀態資訊。

2. 設定 Gazebo 物體初始位置

   • 如果你希望在啟動 Gazebo 時初始化機械臂的位姿，可以透過 rosparam 載入這份 YAML：

     bash
     rosparam load model_states.yaml
     

3. 透過 /gazebo/set_model_state 來設定物體位置

   • 你可以將這個 YAML 轉換成 gazebo_msgs/ModelState 訊息，然後透過 rostopic pub 來更新機器人的位置：

     bash
     rostopic pub /gazebo/set_model_state gazebo_msgs/ModelState "model_name: 'robot_arm_urdf'
     pose:
       position:
         x: 0.0
         y: 0.0
         z: 0.1
       orientation:
         x: 0.0
         y: 0.0
         z: 0.0
         w: 1.0"
     

   • 這會把 robot_arm_urdf 移動到 z=0.1 的位置。

---

3️⃣ 如何使用這份 YAML 來控制機器人？

✅ 1. 使用 rosparam load 來載入 YAML

bash
rosparam load model_states.yaml
rosparam get /name  # 查看載入的數據

這樣可以在 ROS 內讀取 Gazebo 內的機器人狀態。

---

✅ 2. 在 Python 腳本內讀取並發布 Gazebo ModelState

如果你希望在 Python 內讀取 YAML，然後發送 Gazebo 位置，可以使用：

📌 set_model_state.py

python
#!/usr/bin/env python3
import rospy
import yaml
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import SetModelState

# 讀取 YAML 檔案
with open("model_states.yaml", 'r') as file:
    model_data = yaml.safe_load(file)

rospy.init_node("set_gazebo_state")

# 連接 Gazebo 的 `/gazebo/set_model_state` 服務
rospy.wait_for_service('/gazebo/set_model_state')
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)

# 依序設定每個模型的位置
for i in range(len(model_data["name"])):
    state = ModelState()
    state.model_name = model_data["name"][i]
    state.pose.position.x = model_data["pose"][0]["position"]["x"]
    state.pose.position.y = model_data["pose"][0]["position"]["y"]
    state.pose.position.z = model_data["pose"][0]["position"]["z"]
    state.pose.orientation.x = model_data["pose"][0]["orientation"]["x"]
    state.pose.orientation.y = model_data["pose"][0]["orientation"]["y"]
    state.pose.orientation.z = model_data["pose"][0]["orientation"]["z"]
    state.pose.orientation.w = model_data["pose"][0]["orientation"]["w"]

    # 發送新的位姿
    set_model_state(state)
    rospy.sleep(0.5)

print("Gazebo Model State 更新完成！")

✅ 執行 Python 控制

bash
python3 set_model_state.py

這會 讓 Gazebo 內的機器人根據 YAML 內的資料調整位置。

---

📌 總結

項目	功能
name	Gazebo 內的模型名稱（如 robot_arm_urdf::base_link）
pose	定義機器人的初始位置與方向
twist	設定機器人的速度（目前為 0）
rosparam load	讀取 YAML 並儲存為 ROS 參數
set_model_state	讓 Gazebo 根據 YAML 內的資訊更新機器人位置

🚀 這份 YAML 主要用於 Gazebo 內的機器人狀態設定，並可透過 rosparam 或 gazebo_msgs/ModelState 來控制機器人！ 🎯