對先前提供的 joint_state_publisher.py 和 robot_state_publisher.py 的改進版本，加入鍵盤實時調整關節角度的功能。
這些節點將允許使用者通過鍵盤輸入控制機器人手臂的靜態姿態，並在 RViz 中即時顯示變化。
為了實現鍵盤控制，我們將使用 Python 的 keyboard 模組（需安裝：pip install keyboard），並在背景執行緒中監聽按鍵事件。
robot_state_publisher.py 的邏輯保持不變，只需確保它能接收並處理來自改進版 joint_state_publisher.py 的關節狀態。
1. 改進版 joint_state_publisher.py
程式碼：joint_state_publisher_with_keyboard.py
python
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
import threading
import keyboard # 需要安裝：pip install keyboard
class JointStateController:
def __init__(self):
# 初始化 ROS 節點
rospy.init_node('joint_state_publisher_with_keyboard', anonymous=True)
# 創建發布者
self.pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
# 定義關節名稱（假設手臂有兩個關節）
self.joint_names = ['joint_1', 'joint_2']
# 初始化關節角度（單位：弧度）
self.joint_positions = [0.0, 0.0]
# 角度增量（每次按鍵改變的角度）
self.angle_step = 0.1 # 約 5.7 度
# 啟動鍵盤監聽執行緒
self.start_keyboard_listener()
# 開始發布關節狀態
self.publish_joint_states()
def start_keyboard_listener(self):
# 在背景執行緒中監聽鍵盤輸入
threading.Thread(target=self.keyboard_control, daemon=True).start()
def keyboard_control(self):
print("鍵盤控制說明：")
print(" w/s: 增加/減少 joint_1 角度")
print(" e/d: 增加/減少 joint_2 角度")
print(" q: 退出")
while not rospy.is_shutdown():
if keyboard.is_pressed('w'):
self.joint_positions[0] += self.angle_step
elif keyboard.is_pressed('s'):
self.joint_positions[0] -= self.angle_step
elif keyboard.is_pressed('e'):
self.joint_positions[1] += self.angle_step
elif keyboard.is_pressed('d'):
self.joint_positions[1] -= self.angle_step
elif keyboard.is_pressed('q'):
rospy.signal_shutdown("User requested shutdown")
break
# 限制角度範圍（例如 -π 到 π）
self.joint_positions[0] = max(min(self.joint_positions[0], 3.14), -3.14)
self.joint_positions[1] = max(min(self.joint_positions[1], 3.14), -3.14)
# 短暫休眠，避免過高 CPU 使用率
rospy.sleep(0.05)
def publish_joint_states(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# 創建 JointState 訊息
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = self.joint_names
joint_state.position = self.joint_positions
joint_state.velocity = [0.0, 0.0] # 靜態模型
joint_state.effort = []
# 發布訊息
self.pub.publish(joint_state)
# 顯示當前角度（調試用）
rospy.loginfo(f"Joint angles: {self.joint_positions}")
rate.sleep()
if __name__ == '__main__':
try:
JointStateController()
except rospy.ROSInterruptException:
pass改進說明
- 鍵盤控制：
- w / s：增加/減少 joint_1 的角度。
- e / d：增加/減少 joint_2 的角度。
- q：退出節點。
- 角度限制：將角度限制在 -π 到 π（-180° 到 180°）之間，避免無限增長。
- 執行緒：鍵盤監聽在獨立執行緒中運行，避免阻塞主循環。
- 反饋：每次發布時顯示當前角度，便於調試。
- 安裝依賴：需執行 pip install keyboard，並以 root 權限運行（或在 Linux 中使用 sudo）。
2. 原版 robot_state_publisher.py（不需大改）
robot_state_publisher 的功能保持不變，因為它只需接收 /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):
for joint in self.robot.joints:
if joint.type in ['revolute', 'continuous']:
parent = joint.parent
child = joint.child
origin = joint.origin
angle = self.joint_positions.get(joint.name, 0.0)
quat = quaternion_from_euler(0, 0, angle) # 假設沿 Z 軸旋轉
pos = [origin.xyz[0], origin.xyz[1], origin.xyz[2]] if origin.xyz else [0, 0, 0]
self.broadcaster.sendTransform(
pos, quat, rospy.Time.now(), child, parent
)
if __name__ == '__main__':
try:
CustomRobotStatePublisher()
except rospy.ROSInterruptException:
pass說明
- 無需改動：此節點已能處理來自改進版 joint_state_publisher 的實時關節狀態。
- 假設：關節沿 Z 軸旋轉，若 URDF 使用其他軸，需根據 joint.axis 調整 quaternion_from_euler。
運行步驟
- 準備環境：
- 啟動 ROS：roscore。
- 載入 URDF（假設使用 display.launch 或手動設置）：bash
rosparam set /robot_description -t $(rospack find robot_arm_urdf)/urdf/robot_arm_urdf.urdf - 啟動 RViz：rosrun rviz rviz，添加 RobotModel 和 TF。
- 啟動節點：
- 運行 robot_state_publisher：bash
rosrun <your_package> robot_state_publisher.py - 運行鍵盤控制節點（可能需 sudo）：bash
sudo rosrun <your_package> joint_state_publisher_with_keyboard.py
- 鍵盤控制：
- 按 w / s 調整 joint_1。
- 按 e / d 調整 joint_2。
- 按 q 退出。
- 在 RViz 中觀察手臂姿態實時變化。
效果
- 實時調整：按下按鍵時，關節角度以 0.1 弧度（約 5.7°）為步長增減，RViz 即時更新模型姿態。
- 靜態特性：無物理模擬，姿態完全由鍵盤輸入決定。
- 限制：
- keyboard 模組在 Linux 上可能需要 root 權限。
- 若鍵盤反應遲緩，可調整 rospy.sleep(0.05) 或 rate。
替代方案（無需 keyboard 模組）
若不希望依賴 keyboard 模組，可使用終端輸入方式：
程式碼：joint_state_publisher_with_input.py
python
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
import threading
class JointStateController:
def __init__(self):
rospy.init_node('joint_state_publisher_with_input', anonymous=True)
self.pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
self.joint_names = ['joint_1', 'joint_2']
self.joint_positions = [0.0, 0.0]
self.angle_step = 0.1
threading.Thread(target=self.user_input, daemon=True).start()
self.publish_joint_states()
def user_input(self):
print("輸入命令：")
print(" 1/2: 增加/減少 joint_1")
print(" 3/4: 增加/減少 joint_2")
print(" q: 退出")
while not rospy.is_shutdown():
cmd = input("輸入命令: ")
if cmd == '1':
self.joint_positions[0] += self.angle_step
elif cmd == '2':
self.joint_positions[0] -= self.angle_step
elif cmd == '3':
self.joint_positions[1] += self.angle_step
elif cmd == '4':
self.joint_positions[1] -= self.angle_step
elif cmd == 'q':
rospy.signal_shutdown("User requested shutdown")
break
self.joint_positions[0] = max(min(self.joint_positions[0], 3.14), -3.14)
self.joint_positions[1] = max(min(self.joint_positions[1], 3.14), -3.14)
def publish_joint_states(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = self.joint_names
joint_state.position = self.joint_positions
joint_state.velocity = [0.0, 0.0]
joint_state.effort = []
self.pub.publish(joint_state)
rospy.loginfo(f"Joint angles: {self.joint_positions}")
rate.sleep()
if __name__ == '__main__':
try:
JointStateController()
except rospy.ROSInterruptException:
pass說明
- 終端控制：通過輸入數字（1-4）調整角度，無需額外模組。
- 缺點：非實時，需每次輸入並按回車。
總結
- 鍵盤版本：提供實時控制，適合動態調整。
- 輸入版本：簡單但非即時，適合測試。
- RViz 可視化：兩版本均可與 robot_state_publisher.py 配合，實現靜態姿態的實時更新。
沒有留言:
張貼留言