(Robot_arm)py3 code node 模擬 joint_state_publisher 和 robot_state_publisher 的功能

 以下是使用 Python 撰寫 ROS 節點的範例程式碼，模擬 joint_state_publisher 和 robot_state_publisher 的功能，並實現靜態模型的可視化控制。

這些節點將發布關節狀態並計算 TF，從而在 RViz 中顯示機器人模型的姿態。

假設我們控制一個簡單的雙關節手臂模型（類似您提到的 robot_arm_urdf），並允許使用者通過程式碼調整關節角度。

---

環境假設

• ROS 環境已設置（例如 roscore 已運行）。
• URDF 文件（robot_arm_urdf.urdf）已載入到參數服務器（可通過 display.launch 或手動設置）。
• RViz 已啟動並配置為顯示 TF 和機器人模型。

---

1. 模擬 joint_state_publisher 的 Python 節點

這個節點發布 /joint_states 話題，模擬關節狀態。

程式碼：joint_state_publisher.py

python

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import JointState
import math

def publish_joint_states():
    # 初始化 ROS 節點
    rospy.init_node('custom_joint_state_publisher', anonymous=True)
    
    # 創建發布者，發布到 /joint_states 話題
    pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
    
    # 設置發布頻率（例如 10 Hz）
    rate = rospy.Rate(10)
    
    # 定義關節名稱（假設手臂有兩個關節）
    joint_names = ['joint_1', 'joint_2']
    
    # 初始化角度（單位：弧度）
    joint_positions = [0.0, 0.0]  # 初始為 0 度
    
    while not rospy.is_shutdown():
        # 創建 JointState 訊息
        joint_state = JointState()
        joint_state.header.stamp = rospy.Time.now()
        joint_state.name = joint_names
        joint_state.position = joint_positions
        joint_state.velocity = [0.0, 0.0]  # 靜態模型，速度為 0
        joint_state.effort = []  # 無力數據
        
        # 發布訊息
        pub.publish(joint_state)
        
        # 示例：模擬關節角度變化（可替換為使用者輸入）
        joint_positions[0] = math.sin(rospy.get_time())  # joint_1 隨時間正弦變化
        joint_positions[1] = math.cos(rospy.get_time())  # joint_2 隨時間餘弦變化
        
        # 控制發布頻率
        rate.sleep()

if __name__ == '__main__':
    try:
        publish_joint_states()
    except rospy.ROSInterruptException:
        pass

說明

• 功能：發布 JointState 訊息到 /joint_states，包含兩個關節（joint_1 和 joint_2）的角度。
• 靜態控制：示例中使用了正弦和餘弦函數模擬變化，但您可以改為固定值（如 [0.5, 1.0]）或通過輸入控制。
• 執行：
  1. 儲存為 joint_state_publisher.py。
  2. 賦予執行權限：chmod +x joint_state_publisher.py。
  3. 運行：rosrun <your_package> joint_state_publisher.py。

---

2. 模擬 robot_state_publisher 的 Python 節點

這個節點根據 URDF 和 /joint_states 計算並發布 TF。

程式碼：robot_state_publisher.py

python

#!/usr/bin/env python

import rospy
import tf
from sensor_msgs.msg import JointState
from urdf_parser_py.urdf import URDF
from tf.transformations import quaternion_from_euler

class CustomRobotStatePublisher:
    def __init__(self):
        # 初始化 ROS 節點
        rospy.init_node('custom_robot_state_publisher', anonymous=True)
        
        # 從參數服務器載入 URDF
        robot_description = rospy.get_param('/robot_description')
        self.robot = URDF.from_xml_string(robot_description)
        
        # 創建 TF 廣播器
        self.broadcaster = tf.TransformBroadcaster()
        
        # 訂閱 /joint_states 話題
        rospy.Subscriber('/joint_states', JointState, self.joint_state_callback)
        
        # 儲存當前關節狀態
        self.joint_positions = {}
        
        # 運行節點
        rospy.spin()

    def joint_state_callback(self, msg):
        # 更新關節狀態
        for name, pos in zip(msg.name, msg.position):
            self.joint_positions[name] = pos
        
        # 發布 TF
        self.publish_transforms()

    def publish_transforms(self):
        # 遍歷 URDF 中的關節
        for joint in self.robot.joints:
            if joint.type in ['revolute', 'continuous']:  # 只處理旋轉關節
                parent = joint.parent
                child = joint.child
                origin = joint.origin
                
                # 從 joint_states 獲取當前角度，無數據則為 0
                angle = self.joint_positions.get(joint.name, 0.0)
                
                # 計算 TF（假設沿 Z 軸旋轉）
                quat = quaternion_from_euler(0, 0, angle)  # Roll, Pitch, Yaw
                pos = [origin.xyz[0], origin.xyz[1], origin.xyz[2]] if origin.xyz else [0, 0, 0]
                
                # 發布 TF
                self.broadcaster.sendTransform(
                    pos, quat, rospy.Time.now(), child, parent
                )

if __name__ == '__main__':
    try:
        CustomRobotStatePublisher()
    except rospy.ROSInterruptException:
        pass

說明

• 功能：從 /joint_states 獲取關節狀態，根據 URDF 計算並發布 TF。
• 依賴：需要 urdf_parser_py 庫（pip install urdf-parser-py）。
• 簡化假設：假設關節沿 Z 軸旋轉，實際 URDF 可能需要根據 axis 屬性調整。
• 執行：
  1. 儲存為 robot_state_publisher.py。
  2. 賦予執行權限：chmod +x robot_state_publisher.py。
  3. 運行：rosrun <your_package> robot_state_publisher.py。

---

靜態可視化的動作控制

如何實現

1. 準備環境：
   • 啟動 roscore。
   • 載入 URDF（例如通過 roslaunch 或 rosparam set /robot_description -t robot_arm_urdf.urdf）。
   • 啟動 RViz：rosrun rviz rviz，添加 RobotModel 和 TF 顯示。
2. 運行節點：
   • 啟動 robot_state_publisher.py：處理 TF。
   • 啟動 joint_state_publisher.py：發布關節狀態。
3. 控制動作：
   • 修改 joint_state_publisher.py 中的 joint_positions：
     • 固定值：例如 joint_positions = [0.5, 1.0]（弧度），手臂靜止於特定姿態。
     • 動態值：保留示例中的正弦/餘弦變化，模擬平滑運動。
     • 使用者輸入：添加輸入邏輯，例如：
       python

       joint_positions[0] = float(input("Enter angle for joint_1 (radians): "))
       joint_positions[1] = float(input("Enter angle for joint_2 (radians): "))

RViz 中的效果

• 靜態姿態：設定固定角度後，RViz 顯示手臂靜止於該姿態。
• 偽動態效果：使用正弦/餘弦變化，RViz 顯示手臂緩慢擺動，但仍屬靜態控制範疇（無物理模擬）。

---

完整運行示例

1. 啟動 ROS：
   bash

   roscore

2. 載入 URDF（假設已準備好 display.launch）：
   bash

   roslaunch robot_arm_urdf display.launch gui:=false

   （這裡禁用 GUI，因為我們用自訂節點替代）
3. 啟動自訂節點：
   bash

   rosrun <your_package> robot_state_publisher.py
   rosrun <your_package> joint_state_publisher.py

4. 啟動 RViz：
   bash

   rosrun rviz rviz

---

靜態可視化的特點

• 無物理模擬：姿態由程式碼直接控制，不涉及重力或碰撞。
• 靈活性：可輕鬆修改 joint_positions 實現不同姿態。
• 簡單性：適合快速驗證 URDF 或展示靜態結構。

ps 可更具體的控制邏輯（例如通過鍵盤實時調整角度）或針對特定 URDF 優化程式碼