import rclpy
from rclpy.node import Node
import can
import threading
import sys
import termios
import tty
import struct
import signal
from std_msgs.msg import Float32, Bool
from geometry_msgs.msg import Point
from rclpy.qos import QoSProfile, ReliabilityPolicy
class ArmController(Node):
def init(self):
super().init('arm_controller')
初始化参数
self.declare_parameter('can_channel', 'can1')
self.declare_parameter('can_bitrate', 500000)
关节1参数
self.declare_parameter('joint1_min', 0.0)
self.declare_parameter('joint1_max', 1.0)
self.declare_parameter('joint1_step', 0.002)
self.declare_parameter('joint1_can_id', 0x10)
关节2参数
self.declare_parameter('joint2_min', 0.0)
self.declare_parameter('joint2_max', 1.0)
self.declare_parameter('joint2_step', 0.002)
self.declare_parameter('joint2_can_id', 0x11)
self.can_channel = self.get_parameter('can_channel').value
关节1参数
self.joint1_min = self.get_parameter('joint1_min').value
self.joint1_max = self.get_parameter('joint1_max').value
self.joint1_step = self.get_parameter('joint1_step').value
self.joint1_can_id = self.get_parameter('joint1_can_id').value
关节2参数
self.joint2_min = self.get_parameter('joint2_min').value
self.joint2_max = self.get_parameter('joint2_max').value
self.joint2_step = self.get_parameter('joint2_step').value
self.joint2_can_id = self.get_parameter('joint2_can_id').value
线程安全锁
self.lock = threading.Lock()
self.running = True
当前目标位置 [关节1, 关节2]
self.target_joint1 = 0.2
self.target_joint2 = 0.2
统计信息
self.send_count = 0
self.error_count = 0
初始化CAN总线
self.init_can_bus()
创建ROS2发布者
qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
self.status_pub = self.create_publisher(Point, 'arm/current_position', qos)
self.error_pub = self.create_publisher(Bool, 'arm/error_status', qos)
创建定时器，定期发布状态
self.timer = self.create_timer(0.1, self.publish_status)
设置信号处理
signal.signal(signal.SIGINT, self.signal_handler)
self.get_logger().info(f'Arm Controller initialized on {self.can_channel}')
self.get_logger().info(f'Joint1 Range: [{self.joint1_min}, {self.joint1_max}], Step: {self.joint1_step}')
self.get_logger().info(f'Joint2 Range: [{self.joint2_min}, {self.joint2_max}], Step: {self.joint2_step}')
def init_can_bus(self):
"""初始化CAN总线，带重试机制"""
retry_count = 3
for i in range(retry_count):
try:
self.bus = can.interface.Bus(
channel=self.can_channel,
bustype='socketcan'
)
self.get_logger().info(f'CAN bus connected to {self.can_channel}')
return
except Exception as e:
self.get_logger().error(f'CAN bus init failed (attempt {i+1}/{retry_count}): {e}')
if i < retry_count - 1:
import time
time.sleep(1)
else:
raise RuntimeError(f'Cannot connect to CAN bus: {e}')
def send_target(self, joint_id, mode, x):
"""发送CAN指令到指定关节
joint_id: 1 或 2
mode: 1=绝对位置, 2=相对位置
x: 位置值
"""
选择对应的CAN ID和边界
if joint_id == 1:
can_id = self.joint1_can_id
min_val = self.joint1_min
max_val = self.joint1_max
else:
can_id = self.joint2_can_id
min_val = self.joint2_min
max_val = self.joint2_max
边界检查
if mode == 1: # 绝对位置模式
x = max(min_val, min(max_val, x))
try:
打包数据
data = bytearray(8)
data[0] = mode
struct.pack_into('<f', data, 1, x)
计算校验和（简单的XOR校验）
checksum = 0
for b in data[:7]:
checksum ^= b
data[7] = checksum
创建CAN消息
msg = can.Message(
arbitration_id=can_id,
data=data,
is_extended_id=False
)
发送消息，带超时
self.bus.send(msg, timeout=0.1)
更新统计
with self.lock:
self.send_count += 1
记录发送日志
self.get_logger().debug(f'Sent to Joint{joint_id}: mode={mode}, x={x:.3f}')
except can.CanError as e:
with self.lock:
self.error_count += 1
self.get_logger().error(f'CAN send error for Joint{joint_id}: {e}')
发布错误状态
error_msg = Bool()
error_msg.data = True
self.error_pub.publish(error_msg)
except Exception as e:
self.get_logger().error(f'Unexpected error in send_target: {e}')
def keyboard_control(self):
"""键盘控制线程"""
self.get_logger().info('Keyboard control started')
print("\n=== Arm Controller Keyboard Control (2 DOF) ===")
print("Controls:")
print(" W/S: Joint1 + / -")
print(" O/L: Joint2 + / -")
print(" A/D: Joint1 large step +/-")
print(" K/;: Joint2 large step +/-")
print(" h: Help")
print(" s: Show status")
print(" q: Quit keyboard mode")
print("===============================================")
while self.running:
try:
ch = self.get_key()
if ch == 'q':
self.get_logger().info('Keyboard control stopped by user')
break
elif ch == 'h':
self.print_help()
elif ch == 's':
self.print_status()
Joint1 控制
elif ch == 'w': # Joint1 增加
with self.lock:
new_x = self.target_joint1 + self.joint1_step
if new_x <= self.joint1_max:
self.target_joint1 = new_x
self.send_target(1, 2, self.target_joint1)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [+J1]", end='')
else:
print(f"\rJoint1 max limit reached: {self.joint1_max:.3f}", end='')
elif ch == 's': # Joint1 减少
with self.lock:
new_x = self.target_joint1 - self.joint1_step
if new_x >= self.joint1_min:
self.target_joint1 = new_x
self.send_target(1, 2, self.target_joint1)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [-J1]", end='')
else:
print(f"\rJoint1 min limit reached: {self.joint1_min:.3f}", end='')
elif ch == 'd': # Joint1 大步长增加
with self.lock:
new_x = self.target_joint1 + self.joint1_step * 10
if new_x <= self.joint1_max:
self.target_joint1 = new_x
self.send_target(1, 2, self.target_joint1)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [+J1 big]", end='')
else:
print(f"\rJoint1 max limit reached: {self.joint1_max:.3f}", end='')
elif ch == 'a': # Joint1 大步长减少
with self.lock:
new_x = self.target_joint1 - self.joint1_step * 10
if new_x >= self.joint1_min:
self.target_joint1 = new_x
self.send_target(1, 2, self.target_joint1)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [-J1 big]", end='')
else:
print(f"\rJoint1 min limit reached: {self.joint1_min:.3f}", end='')
Joint2 控制
elif ch == 'o': # Joint2 增加
with self.lock:
new_x = self.target_joint2 + self.joint2_step
if new_x <= self.joint2_max:
self.target_joint2 = new_x
self.send_target(2, 2, self.target_joint2)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [+J2]", end='')
else:
print(f"\rJoint2 max limit reached: {self.joint2_max:.3f}", end='')
elif ch == 'l': # Joint2 减少
with self.lock:
new_x = self.target_joint2 - self.joint2_step
if new_x >= self.joint2_min:
self.target_joint2 = new_x
self.send_target(2, 2, self.target_joint2)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [-J2]", end='')
else:
print(f"\rJoint2 min limit reached: {self.joint2_min:.3f}", end='')
elif ch == ';': # Joint2 大步长增加
with self.lock:
new_x = self.target_joint2 + self.joint2_step * 10
if new_x <= self.joint2_max:
self.target_joint2 = new_x
self.send_target(2, 2, self.target_joint2)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [+J2 big]", end='')
else:
print(f"\rJoint2 max limit reached: {self.joint2_max:.3f}", end='')
elif ch == 'k': # Joint2 大步长减少
with self.lock:
new_x = self.target_joint2 - self.joint2_step * 10
if new_x >= self.joint2_min:
self.target_joint2 = new_x
self.send_target(2, 2, self.target_joint2)
print(f"\rJoint1: {self.target_joint1:.3f} Joint2: {self.target_joint2:.3f} [-J2 big]", end='')
else:
print(f"\rJoint2 min limit reached: {self.joint2_min:.3f}", end='')
except Exception as e:
self.get_logger().error(f'Keyboard control error: {e}')
def get_key(self):
"""获取键盘输入（原始模式）"""
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(fd)
ch = sys.stdin.read(1)
处理方向键（3个字符的转义序列）- 保留但可能不使用
if ch == '\x1b':
ch2 = sys.stdin.read(1)
if ch2 == '[':
ch3 = sys.stdin.read(1)
return f'\x1b[{ch3}'
return ch + ch2
return ch
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
def print_help(self):
"""打印帮助信息"""
print("\n=== Help ===")
print("Joint1 controls:")
print(" W/S: Small step +/-")
print(" A/D: Large step +/-")
print("Joint2 controls:")
print(" O/L: Small step +/-")
print(" K/;: Large step +/-")
print("s: Show status")
print("h: Show this help")
print("q: Quit keyboard mode")
print("In main thread, you can input absolute positions as: joint1,joint2")
print("Example: 0.5,0.3")
print("============")
def print_status(self):
"""打印当前状态"""
with self.lock:
print(f"\n=== Status ===")
print(f"Joint1 position: {self.target_joint1:.3f} Range: [{self.joint1_min:.3f}, {self.joint1_max:.3f}]")
print(f"Joint2 position: {self.target_joint2:.3f} Range: [{self.joint2_min:.3f}, {self.joint2_max:.3f}]")
print(f"Step sizes - J1: {self.joint1_step:.3f}, J2: {self.joint2_step:.3f}")
print(f"CAN channel: {self.can_channel}")
print(f"CAN IDs - J1: 0x{self.joint1_can_id:02X}, J2: 0x{self.joint2_can_id:02X}")
print(f"Messages sent: {self.send_count}")
print(f"Errors: {self.error_count}")
print("==============")
def publish_status(self):
"""定期发布状态到ROS2"""
with self.lock:
msg = Point()
msg.x = self.target_joint1
msg.y = self.target_joint2
msg.z = 0.0 # 未使用
self.status_pub.publish(msg)
def signal_handler(self, sig, frame):
"""处理Ctrl+C信号"""
self.get_logger().info('Shutdown signal received')
self.running = False
self.cleanup()
sys.exit(0)
def cleanup(self):
"""清理资源"""
self.get_logger().info('Cleaning up...')
try:
if hasattr(self, 'bus') and self.bus:
self.bus.shutdown()
self.get_logger().info('CAN bus closed')
except Exception as e:
self.get_logger().error(f'Error during cleanup: {e}')
def main(args=None):
初始化ROS2
rclpy.init(args=args)
创建控制器实例
ctrl = ArmController()
启动键盘控制线程
keyboard_thread = threading.Thread(target=ctrl.keyboard_control, daemon=True)
keyboard_thread.start()
创建单独的输入线程来处理主线程输入
def input_thread_func():
print("\n=== Main Control Mode ===")
print("Enter absolute positions (format: joint1,joint2) or 'q' to quit, 's' for status:")
print("Example: 0.5,0.3")
while ctrl.running:
try:
user_input = input().strip()
if user_input.lower() == 'q':
ctrl.running = False
break
elif user_input.lower() == 's':
ctrl.print_status()
continue
elif user_input == '':
continue
解析输入 - 支持逗号分隔的两个值
parts = user_input.split(',')
if len(parts) != 2:
print("Error: Please enter two values separated by comma (e.g., 0.5,0.3)")
continue
x1 = float(parts[0].strip())
x2 = float(parts[1].strip())
边界检查
if x1 < ctrl.joint1_min or x1 > ctrl.joint1_max:
print(f"Error: Joint1 position out of range [{ctrl.joint1_min}, {ctrl.joint1_max}]")
continue
if x2 < ctrl.joint2_min or x2 > ctrl.joint2_max:
print(f"Error: Joint2 position out of range [{ctrl.joint2_min}, {ctrl.joint2_max}]")
continue
更新位置并发送
with ctrl.lock:
ctrl.target_joint1 = x1
ctrl.target_joint2 = x2
ctrl.send_target(1, 1, x1) # 绝对位置模式
ctrl.send_target(2, 1, x2) # 绝对位置模式
print(f"Sent absolute positions: Joint1={x1:.3f}, Joint2={x2:.3f}")
except ValueError:
print(f"Error: Invalid input '{user_input}'. Please enter numbers separated by comma.")
except EOFError:
break
except Exception as e:
ctrl.get_logger().error(f'Input error: {e}')
input_thread = threading.Thread(target=input_thread_func, daemon=True)
input_thread.start()
try:
运行ROS2 spin
rclpy.spin(ctrl)
except KeyboardInterrupt:
ctrl.get_logger().info('Keyboard interrupt received')
finally:
ctrl.cleanup()
rclpy.shutdown()
if name == 'main':
main()