# arm_controller_ws

**Repository Path**: CeasarSmj/arm_controller_ws

## Basic Information

- **Project Name**: arm_controller_ws
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2025-03-21
- **Last Updated**: 2025-11-29

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 控制器

## 简述
在RM工程开发过程中，需要对于一个7自由度的机械臂进行控制。  

由于单独控制关节的方案不直观，也不方便让人类使用，所以我们决定做一个自定义控制器可以直接指定末端执行器的位姿，而中间的复杂计算交给计算机进行。  

控制器分为手持端和计算端组成：
- **手持端**: 由打印件+导线+发光二极管+电阻+干电池组成的座标架，六个发光二极管作为识别特征，可以反映整个座标架的位姿。
- **计算端**: 由NUC+相机+USB转CAN组成的系统，可以捕获座标架的图像并进行识别和解算，最后通过串口把关节角发送给下位机。

同时控制器也会提供一个解算服务，根据来自串口的位姿计算关节角并反馈。

## 视觉解算
写一个camera_calib来标定相机：从相机获取图像，检测其中的棋盘图片(9x11 2cm)，以0.2的概率从有效图像中采样；点ESC退出后计算并保存参数。

使用yolov8识别发光二极管光点，并使用PNP解算的方式得到相对于相机的位姿。

对于这个位姿进行一定的位姿变换使其转化到机器人坐标系，然后作为末端执行器的期望位姿。

## 逆向运动学解算

## 封装
NUC搭载的系统为Ubuntu20.04，我们决定使用成熟的ROS noetic来进行封装，位于controller_ws。

### 包列表

#### realsense_pkg
相机驱动包
- 发布
  - `image_raw` (图片)：相机原始图像
- 获取途径
  - realsense-sdk : https://github.com/IntelRealSense/realsense-ros
  - realsense-ros-pkg : https://github.com/ros2/realtime_support.git#ros2-dashing-devel"

#### target_detect_pkg
视觉解算节点，基于开源的yolov5/yolov8包
- 接收
  - `image_raw` (图片)：相机原始图像
- 发布
  - `detected_point` (float64[4])：识别到点中心坐标和类型
- 获取途径
  - yolov8 : https://ultralytics.com

#### pnp_solve_pkg
pnp解算节点，负责解算和位姿变换
- 接收
  - `detected_point` (float64[4])：识别到点中心坐标和类型
- 发布
  - `solve_request_from_vision` (Twist)：逆向运动学解算请求
- 附加脚本
  - `camera_calib.py`：相机标定脚本

#### robot_solver_pkg
机械臂解算节点，调用 python 的roboticstoolbox 
- 服务： arm_control_service 正向运动学解算服务
  - 请求： (float64[7] ：当前关节角 , Twist：末端执行器位姿变化量)
  - 返回： Twist：关节角
  - 过程：接收关节角和末端执行器位姿变化量，使用 roboticstoolbox 进行正向解算，加上变化量后根据新的末端位姿进行逆向解算，然后返回关节角。
> 注：上行串口需求优先，遇到上行串口需求则暂停处理视觉请求2秒

#### middle_coding_pkg
中继节点，用于数据包编解码
- 接收
  - `can_upward_topic` (can_msg)：上行串口话题
    - frame id = 6时表示目前关节角（分别是两个包）
    - frame id = 7时表示末端执行器位姿的变化量
  - `joint_angle_result` (float64[])：关节角
- 发布
  - `can_dwonward_topic` (can_msg)：下行串口话题
    - frame id = 5, 内容为关节角（分成两个包发送）
- 异步存储 关节角和末端执行器位姿变化量（要求为三个连续的包），然后在 can_upward_callback 中调用 `arm_control_service` 服务进行解算后编码发送到 `can_downward_topic`


#### usb_can_pkg
串口通信节点
- 发布
  - `can_upward_topic` (can_msg)：上行串口话题
- 接收
  - `can_dwonward_topic` (can_msg)：下行串口话题


## 资料
1. 适用于 ros noetic 的 yolov5/yolov8 ros包
2. 适用于 ros noetic 的机器人学解算包：moveit


## 通信
通过 usb_can_pkg 下的 serial_can_node 来进行数据收发.
依赖项: roscpp rospy serial std_msgs can_msgs
包含节点:
- serial_can_node: 数据 接收+发送
- serial_can_recv_node: 数据 接收
- serial_can_send_node: 数据 发送
包含launch文件:
- usb_can_send.launch
- usb_can_recv.launch
- usb_can_node.launch

launch 文件参数:
- serial_port: 串口设备路径(默认 /dev/ttyUSB0)
- baudrate: 波特率(默认 115200)
- send_topic: 发送话题(默认 /can_send)
- recv_topic: 接收话题(默认 /can_recv)


数据包结构:
```c
typedef struct {
    int8_t head; // 0xAA

    int8_t dlc; // 数据长度 + 0xC0

    int8_t id_low;
    int8_t id_high;  

    int8_t data[8];
    int8_t end; // 0x55
} can_frame;

```


## 测试
给 middle_coding_node 发送消息:
```shell
rostopic pub -r 1 /joint_angle_result std_msgs/Float64MultiArray "data: [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]"
```