# ptz_controller

**Repository Path**: ccpdead/ptz_controller

## Basic Information

- **Project Name**: ptz_controller
- **Description**: 云台控制程序
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-01-09
- **Last Updated**: 2026-01-10

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# PTZ Controller - RobStride电机控制器

基于ROS2的RobStride电机控制包，通过CAN总线实现电机的CSP位置控制模式。

## 功能特性

- ✅ 支持多电机同时控制
- ✅ CSP（循环同步位置）模式控制
- ✅ 通过ROS2 topic接收控制指令
- ✅ 实时发布电机反馈状态
- ✅ 电机ID通过消息header.seq传递
- ✅ 支持RobStride全系列电机型号

## 系统要求

- ROS2 (Humble/Iron/Rolling)
- Linux SocketCAN支持
- C++17编译器

## 消息定义

### 输入消息：PosCSP.msg
```
std_msgs/Header header  # header.seq = 电机ID
float32 speed           # 目标速度 (rad/s)
float32 angle           # 目标角度 (rad)
```

### 输出消息：motoRec.msg
```
std_msgs/Header header  # header.seq = 电机ID
float32 position_       # 当前位置 (rad)
float32 velocity_       # 当前速度 (rad/s)
float32 torque_         # 当前扭矩 (Nm)
float32 temperture_     # 当前温度 (°C)
```

## 编译

```bash
cd ~/fastlivo_ws
colcon build --packages-select ptz_controller
source install/setup.bash
```

## 配置

编辑配置文件 `config/motor_config.yaml`：

```yaml
/**:
  ros__parameters:
    can_interface: "can0"   # CAN接口名称
    master_id: 253          # 主机ID (0xFD)
    motor_ids: [1, 2]       # 电机ID列表
    motor_types: [3, 3]     # 电机型号列表
```

**电机型号对照表：**
- 0: ROBSTRIDE_00
- 1: ROBSTRIDE_01
- 2: ROBSTRIDE_02
- 3: ROBSTRIDE_03 (常用)
- 4: ROBSTRIDE_04
- 5: ROBSTRIDE_05
- 6: ROBSTRIDE_06

## 运行

### 方法1：使用launch文件
```bash
ros2 launch ptz_controller motor_controller.launch.py
```

### 方法2：直接运行节点
```bash
ros2 run ptz_controller motor_controller_node --ros-args --params-file config/motor_config.yaml
```

## 使用示例

### 发送控制指令（Python）

```python
import rclpy
from rclpy.node import Node
from ptz_controller.msg import PosCSP

class MotorCommandPublisher(Node):
    def __init__(self):
        super().__init__('motor_command_publisher')
        self.publisher = self.create_publisher(PosCSP, 'motor_pos_csp_cmd', 10)
        
    def send_command(self, motor_id, speed, angle):
        msg = PosCSP()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.seq = motor_id  # 电机ID
        msg.speed = speed          # rad/s
        msg.angle = angle          # rad
        self.publisher.publish(msg)
        self.get_logger().info(f'Sent to motor {motor_id}: speed={speed}, angle={angle}')

def main():
    rclpy.init()
    node = MotorCommandPublisher()
    
    # 示例：控制电机1转到1.57弧度（90度），速度10 rad/s
    node.send_command(motor_id=1, speed=10.0, angle=1.57)
    
    rclpy.shutdown()

if __name__ == '__main__':
    main()
```

### 命令行发送控制指令

```bash
# 控制电机ID=1，速度5 rad/s，角度3.14 rad（180度）
ros2 topic pub --once /motor_pos_csp_cmd ptz_controller/msg/PosCSP \
  "{header: {seq: 1}, speed: 5.0, angle: 3.14}"
```

### 查看电机反馈

```bash
# 实时查看所有电机反馈
ros2 topic echo /motor_feedback

# 查看特定电机（过滤器需要自己实现）
ros2 topic echo /motor_feedback
```

## Topic列表

| Topic名称 | 消息类型 | 方向 | 说明 |
|-----------|---------|------|------|
| `/motor_pos_csp_cmd` | PosCSP | 订阅 | 接收CSP控制指令 |
| `/motor_feedback` | motoRec | 发布 | 发布电机状态反馈 |

## 故障排除

### 1. CAN接口未找到
```bash
# 检查CAN接口状态
ip link show can0

# 启动CAN接口（波特率1Mbps）
sudo ip link set can0 type can bitrate 1000000
sudo ip link set can0 up
```

### 2. 电机无响应
- 检查CAN线缆连接
- 确认电机ID配置正确
- 检查电机供电是否正常
- 查看节点日志：`ros2 run ptz_controller motor_controller_node --ros-args --log-level debug`

### 3. 编译错误
```bash
# 清理并重新编译
rm -rf build install log
colcon build --packages-select ptz_controller --cmake-clean-cache
```

## 性能参数

- **控制频率**：取决于消息发布频率（建议 ≥100Hz）
- **CAN通信速率**：1 Mbps
- **反馈延迟**：< 5ms

## 注意事项

⚠️ **安全警告**
1. 首次运行前确保电机处于安全位置
2. CSP模式需要连续发送指令，否则电机会保持最后位置
3. 建议先在低速下测试（speed < 5 rad/s）
4. 电机温度超过80°C时会触发保护

## 开发者信息

- 基于RobStride电机CAN通信协议
- 使用Linux SocketCAN实现底层通信
- 支持扩展其他控制模式（PP、速度模式、电流模式等）

## 许可证

Apache-2.0