AimBot/Vision
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: initial ROS2 vision pipeline with gimbal control and nginx config

Browse Source 
- camera_node (driver_pkg): Hikrobot MvCamera SDK image capture
- vision_node (algorithm_pkg): Otsu threshold + contour detection + target tracking
- web_node (application_pkg): HTTP server with MJPEG preview and mode switch API
- gimbal_node (gimbal_control_pkg): UART serial communication per gimbal protocol
- bringup_pkg: launch file for full pipeline
- config: nginx reverse-proxy for MJPEG streaming and API routing
This commit is contained in:
dev01
2026-05-31 22:10:12 +08:00
commit d506e01188
51 changed files with 2457 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+28
View File
@@ -0,0 +1,28 @@
# ROS2 colcon build artifacts
workspace/build/
workspace/install/
workspace/log/
# Python
__pycache__/
*.py[cod]
*.egg-info/
*.egg
# IDE
.vscode/
.idea/
*.swp
*.swo
# OS
.DS_Store
Thumbs.db
# Environment
.env
*.log
# Claude
.claude/
CLAUDE.md
config/nginx-aimbot.conf
+43
View File
@@ -0,0 +1,43 @@
# Nginx configuration for Aimbot vision pipeline
# Deploy to: /etc/nginx/sites-available/aimbot
# Enable: ln -s /etc/nginx/sites-available/aimbot /etc/nginx/sites-enabled/
server {
listen 80;
server_name _;
# 前端静态文件 (Vue3 构建产物)
root /var/www/aimbot;
index index.html;
# SPA 路由回退 — 所有非文件请求回 index.html
location / {
try_files $uri $uri/ /index.html;
}
# MJPEG 视频流反代 (web_video_server)
location /stream {
proxy_pass http://127.0.0.1:8081/stream;
proxy_http_version 1.1;
proxy_buffering off;
# MJPEG multipart 边界必须保持完整
proxy_set_header Host $host;
proxy_set_header X-Real-IP $remote_addr;
# 长连接支持
proxy_set_header Connection "";
chunked_transfer_encoding off;
# 超时配置 — 视频流需要长连接
proxy_read_timeout 86400s;
proxy_send_timeout 86400s;
}
# 模式切换 API (HTTP → ROS2 web_node)
location /api/ {
proxy_pass http://127.0.0.1:8080/api/;
proxy_set_header Host $host;
proxy_set_header Content-Type application/json;
}
}
docs/与前端对接文档.md
+250
View File
@@ -0,0 +1,250 @@
# 与前端对接文档
## 整体架构
```
┌──────────────────────────────────────────────────────────┐
│ 浏览器 (Vue3 前端) │
│ - 自动模式：观看 MJPEG 视频流 │
│ - 手动模式：键盘遥控 + MJPEG 视频流 │
│ - 调试模式：PID 调参 │
└──────┬──────────────┬──────────────┬─────────────────────┘
│ HTTP │ WebSocket │ HTTP (MJPEG)
▼ ▼ ▼
┌──────────────┐ ┌──────────┐ ┌───────────────────┐
│ Nginx │ │rosbridge │ │ web_video_server │
│ 静态文件 │ │ :9090 │ │ :8081 │
│ 反向代理 │ └────┬─────┘ └────────┬──────────┘
└──────┬───────┘ │ │
│ ▼ ▼
│ ┌────────────────────────────────┐
└─────►│ ROS2 节点 │
│ - /cmd_wheel (轮速控制) │
│ - /cmd_gimbal (云台控制) │
│ - /pid_config (PID参数) │
│ - /api/mode/switch (模式切换) │
└────────────────────────────────┘
```
## 一、Nginx 配置
Nginx 负责三件事：托管 Vue 前端静态文件、反代 MJPEG 视频流、反代 HTTP API。
### 1.1 前端部署
构建产物在 `dist/` 目录，部署到服务器后由 Nginx 直接托管：
```bash
# 本地构建
cd aimbot-web
pnpm build
# 上传到服务器
scp -r dist/* user@server:/var/www/aimbot/
```
### 1.2 Nginx 配置示例
```nginx
server {
listen 80;
server_name your-domain.com;
# 前端静态文件
root /var/www/aimbot;
index index.html;
# SPA 路由回退 — 所有非文件请求回 index.html
location / {
try_files $uri $uri/ /index.html;
}
# MJPEG 视频流反代 (web_video_server)
location /stream {
proxy_pass http://127.0.0.1:8081/stream;
proxy_http_version 1.1;
proxy_buffering off;
# MJPEG multipart 边界必须保持完整
proxy_set_header Host $host;
proxy_set_header X-Real-IP $remote_addr;
# 长连接支持
proxy_set_header Connection "";
chunked_transfer_encoding off;
# 超时配置 — 视频流需要长连接
proxy_read_timeout 86400s;
proxy_send_timeout 86400s;
}
# 模式切换 API (HTTP → ROS2 节点)
location /api/ {
proxy_pass http://127.0.0.1:8080/api/;
proxy_set_header Host $host;
proxy_set_header Content-Type application/json;
}
}
```
### 1.3 关键说明
| 配置项 | 说明 |
|--------|------|
| `proxy_buffering off` | MJPEG 流必须关闭缓冲，否则画面延迟严重 |
| `chunked_transfer_encoding off` | multipart 边界依赖原始编码 |
| `proxy_read_timeout 86400s` | 视频流是长连接，超时必须足够大 |
| `try_files $uri /index.html` | Vue Hash 路由支持，直接访问 /#/manual 等路径 |
---
## 二、ROS2 环境配置
### 2.1 rosbridge (WebSocket 服务)
前端通过 roslibjs 直连 rosbridge WebSocket，不走 Nginx 反代。
```bash
# 安装
sudo apt install ros-${ROS_DISTRO}-rosbridge-server
# 启动 (默认 ws://0.0.0.0:9090)
ros2 run rosbridge_server rosbridge_websocket
```
### 2.2 web_video_server (MJPEG 推流)
```bash
# 安装
sudo apt install ros-${ROS_DISTRO}-web-video-server
# 启动 (默认 http://0.0.0.0:8081)
# 发布摄像头话题后自动生成 /stream?topic=<topic_name>
ros2 run web_video_server web_video_server
```
### 2.3 话题一览
rosbridge 发布/订阅的话题需要与前端 `config.js` 一致：
| 话题 | 消息类型 | 方向 | 用途 |
|------|----------|------|------|
| `/cmd_wheel` | `std_msgs/Float32MultiArray` | 前端→ROS2 | 左右轮速 `[left, right]` |
| `/cmd_gimbal` | `geometry_msgs/Twist` | 前端→ROS2 | 云台角速度 `angular.y`(pitch) `angular.z`(yaw) |
| `/pid_config` | `std_msgs/String` | 前端→ROS2 | PID 参数 JSON |
### 2.4 模式切换 API
前端 HTTP POST → Nginx 反代 → ROS2 节点（用户自行实现 HTTP 端）：
```
POST /api/mode/switch
Content-Type: application/json
{ "mode": "auto" | "manual" | "debug" }
```
此接口需由 ROS2 侧 HTTP 服务（如 FastAPI / Flask / ros2-web-bridge）接收并处理模式切换逻辑。
---
## 三、MJPEG 视频流地址
`web_video_server` 的 URL 规则：`http://<host>:8081/stream?topic=<话题名>`
前端 `config.js` 中配置对应 URL：
```js
// src/config.js
export const MJPEG_AUTO_URL = '/stream?topic=/camera_auto'
export const MJPEG_MANUAL_URL = '/stream?topic=/camera_manual'
```
使用 `/stream` 相对路径时由 Nginx 反代到 `web_video_server`，避免跨域问题。
### 摄像头话题发布示例
```bash
# 确保摄像头话题有数据
ros2 run image_transport republish raw in:=/camera_auto/image_raw out:=/camera_auto/compressed
```
`web_video_server` 自动识别话题类型并推送 MJPEG。
---
## 四、启动顺序
```bash
# 1. 启动 ROS2 核心节点
ros2 launch your_robot bringup.launch.py
# 2. 启动 rosbridge
ros2 run rosbridge_server rosbridge_websocket &
# 3. 启动视频推流
ros2 run web_video_server web_video_server &
# 4. 启动模式切换 HTTP 服务（自行实现）
python3 mode_api_server.py &
# 5. 启动/重载 Nginx
sudo systemctl reload nginx
```
---
## 五、前端 config.js 配置
部署前修改 `src/config.js` 中的地址以匹配实际服务器环境：
```js
// 后端 HTTP API（模式切换）
export const HTTP_API_BASE = '' // 同域代理可留空或填 '/api'
// rosbridge WebSocket
export const ROSBRIDGE_URL = 'ws://192.168.1.100:9090' // 改成 ROS2 主机 IP
// MJPEG 视频流 — 经 Nginx 代理
export const MJPEG_AUTO_URL = '/stream?topic=/camera_auto'
export const MJPEG_MANUAL_URL = '/stream?topic=/camera_manual'
// ROS 话题 — 必须与 ROS2 侧一致
export const CMD_WHEEL_TOPIC = '/cmd_wheel'
export const CMD_GIMBAL_TOPIC = '/cmd_gimbal'
export const PID_CONFIG_TOPIC = '/pid_config'
// 键盘控制参数 — 按实际机器人调整
export const MAX_LINEAR = 0.5 // 最大线速度 m/s
export const MAX_ANGULAR = 1.0 // 最大角速度 rad/s
export const WHEEL_SEPARATION = 0.2 // 轮距 m
export const GIMBAL_PITCH_SPEED = 0.5 // 云台 pitch 角速度 rad/s
export const GIMBAL_YAW_SPEED = 0.5 // 云台 yaw 角速度 rad/s
```
---
## 六、前端 nginx 完整部署命令
```bash
# 构建
cd aimbot-web && pnpm build
# 部署
sudo cp -r dist/* /var/www/aimbot/
sudo cp nginx-aimbot.conf /etc/nginx/sites-available/aimbot
sudo ln -s /etc/nginx/sites-available/aimbot /etc/nginx/sites-enabled/
sudo nginx -t && sudo systemctl reload nginx
```
---
## 七、故障排查
| 现象 | 检查项 |
|------|--------|
| MJPEG 不出画面 | `web_video_server` 是否运行，话题是否有数据，Nginx `proxy_buffering` 是否设为 `off` |
| rosbridge 连不上 | 防火墙是否开放 9090，`ROSBRIDGE_URL` 是否用 `ws://` 协议 |
| 按键无反应 | rosbridge 连接状态是否为"已连接"，话题名是否与 ROS2 侧一致 |
| 模式切换报错 | `/api/mode/switch` 的 HTTP 服务是否实现并运行 |
| 页面空白 | Nginx `try_files` 是否配了 `$uri /index.html` |
docs/云台通信协议指南.md
+190
View File
@@ -0,0 +1,190 @@
# 云台通信协议指南
## 串口配置
| 参数 | 值 |
|---|---|
| 波特率 | 115200 |
| 数据位 | 8 |
| 停止位 | 1 |
| 校验位 | None |
## 帧格式总览
所有帧通过帧头 (SOF) 区分类型，checksum = 帧头到数据末尾前一字节的所有字节累加和取低 8 位。
| 帧头 | 长度 | 用途 |
|---|---|---|
| 0xA5 | 10 | 自动/视觉追踪数据 |
| 0xA6 | 11 | 手动控制数据 |
| 0xA7 | 3 | 命令 |
| 0xA8 | 50 | 调试调参数据 |
---
## 1. 自动/视觉追踪数据帧 0xA5 (10 字节)
**用途**: 视觉识别完成后持续发送跟踪误差。
```
[SOF:0xA5][error_x:4][error_y:4][checksum:1]
```
| 偏移 | 长度 | 类型 | 含义 |
|---|---|---|---|
| 0 | 1 | uint8 | 帧头 0xA5 |
| 1 | 4 | float | 归一化 X 轴误差 |
| 5 | 4 | float | 归一化 Y 轴误差 |
| 9 | 1 | uint8 | checksum |
**Python 写入示例**:
```python
import struct
def send_auto_tracking(ser, error_x: float, error_y: float):
data = bytearray()
data.append(0xA5)
data.extend(struct.pack('<ff', error_x, error_y))
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 2. 手动控制数据帧 0xA6 (11 字节)
**用途**: 手动模式下发送角度目标值。
```
[SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1]
```
| 偏移 | 长度 | 类型 | 含义 |
|---|---|---|---|
| 0 | 1 | uint8 | 帧头 0xA6 |
| 1 | 1 | uint8 | 零力标志 (0=正常, 1=零力) |
| 2 | 4 | float | Yaw 角度参考 (degrees) |
| 6 | 4 | float | Pitch 角度参考 (degrees) |
| 10 | 1 | uint8 | checksum |
**Python 写入示例**:
```python
def send_manual_control(ser, yaw_deg: float, pitch_deg: float, zero_force: int = 0):
data = bytearray()
data.append(0xA6)
data.append(0 if zero_force else 0)
data.extend(struct.pack('<ff', yaw_deg, pitch_deg))
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 3. 命令帧 0xA7 (3 字节)
**用途**: 切换模式和控制激光。
```
[SOF:0xA7][cmd:1][checksum:1]
```
| cmd 值 | 含义 |
|---|---|
| 0x01 | 切换到自动/视觉追踪模式 |
| 0x02 | 切换到手动模式 |
| 0x05 | 切换到调试模式 (失能所有电机) |
| 0x03 | 激光开 |
| 0x04 | 激光关 |
**Python 写入示例**:
```python
CMD_AUTO = 0x01
CMD_MANUAL = 0x02
CMD_LASER_ON = 0x03
CMD_LASER_OFF = 0x04
CMD_DEBUG = 0x05
def send_command(ser, cmd: int):
data = bytearray([0xA7, cmd])
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 4. 调试调参帧 0xA8 (50 字节)
**用途**: 在调试模式下发送 PID 参数进行调参。发送后固件会立即生效。
```
[SOF:0xA8]
[vision_yaw_Kp:4][vision_yaw_Ki:4][vision_yaw_Kd:4]
[vision_pitch_Kp:4][vision_pitch_Ki:4][vision_pitch_Kd:4]
[motor_yaw_Kp:4][motor_yaw_Ki:4][motor_yaw_Kd:4]
[motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4]
[checksum:1]
```
| 偏移 | 长度 | 类型 | 含义 |
|---|---|---|---|
| 0 | 1 | uint8 | 帧头 0xA8 |
| 1 | 4 | float | 视觉环 Yaw Kp |
| 5 | 4 | float | 视觉环 Yaw Ki |
| 9 | 4 | float | 视觉环 Yaw Kd |
| 13 | 4 | float | 视觉环 Pitch Kp |
| 17 | 4 | float | 视觉环 Pitch Ki |
| 21 | 4 | float | 视觉环 Pitch Kd |
| 25 | 4 | float | 电机角度环 Yaw Kp |
| 29 | 4 | float | 电机角度环 Yaw Ki |
| 33 | 4 | float | 电机角度环 Yaw Kd |
| 37 | 4 | float | 电机角度环 Pitch Kp |
| 41 | 4 | float | 电机角度环 Pitch Ki |
| 45 | 4 | float | 电机角度环 Pitch Kd |
| 49 | 1 | uint8 | checksum |
**Python 写入示例**:
```python
def send_debug_tune(ser,
v_yaw_kp, v_yaw_ki, v_yaw_kd,
v_pitch_kp, v_pitch_ki, v_pitch_kd,
m_yaw_kp, m_yaw_ki, m_yaw_kd,
m_pitch_kp, m_pitch_ki, m_pitch_kd):
data = bytearray()
data.append(0xA8)
data.extend(struct.pack('<fff', v_yaw_kp, v_yaw_ki, v_yaw_kd))
data.extend(struct.pack('<fff', v_pitch_kp, v_pitch_ki, v_pitch_kd))
data.extend(struct.pack('<fff', m_yaw_kp, m_yaw_ki, m_yaw_kd))
data.extend(struct.pack('<fff', m_pitch_kp, m_pitch_ki, m_pitch_kd))
checksum = sum(data) & 0xFF
data.append(checksum)
ser.write(data)
```
---
## 调试流程
1. 上位机发送命令帧 (0xA7, cmd=0x05) 切换到调试模式
2. 固件收到后失能 yaw 和 pitch 电机，日志指示进入调试模式
3. 通过 0xA8 帧写入 PID 参数，参数会**实时生效**：
- 视觉环 PID → 直接写入选中的视觉 PID 实例
- 电机角度环 PID → 通过话题推送到 gimbal 更新电机角度 PID
4. 调试完毕后，发送命令帧 (0xA7, cmd=0x02 手动 或 0x01 自动) 切出调试模式，电机会自动使能
---
## 校验和计算
```python
def calc_checksum(data: bytes) -> int:
return sum(data) & 0xFF # 所有字节累加取低 8 位
```
校验和覆盖范围：**帧头到数据末尾前一字节**的全部字节。接收端在收到数据后也应计算并比对校验和。
vision_example.py
Executable
+397
View File
@@ -0,0 +1,397 @@
# -- coding: utf-8 --
import sys
import platform
import copy
import os
from ctypes import *
import numpy
import cv2
# 配置项
image_area = 1080 * 1440 / 4
threshold_area = image_area*0.01
currentsystem = platform.system()
print(f"current system: {currentsystem}")
if currentsystem == 'Windows':
sys.path.append(os.getenv('MVCAM_COMMON_RUNENV') + "/Samples/Python/MvImport")
else:
# Demo 目录: "/opt/xxx/Samples/平台/OpenCV/ZZ/ ") python接口目录： "/opt/xxx/Samples/平台/Python/MvImport"
sys.path.append("/opt/MVS/Samples/aarch64/Python/MvImport")
print(f"sys.path: {sys.path}")
from MvCameraControl_class import *
# 兼容Python 2.x和3.x的输入处理
if sys.version_info[0] < 3:
# Python 2.x
input_func = raw_input
else:
# Python 3.x
input_func = input
stFrame = POINTER(MV_FRAME_OUT)
fun_ctype = get_platform_functype()
FrameInfoCallBack2 = fun_ctype(None, stFrame, c_void_p, c_bool)
def IsHBPixelFormat(enPixelType = 0):
if enPixelType in (PixelType_Gvsp_HB_Mono8, \
PixelType_Gvsp_HB_Mono10,\
PixelType_Gvsp_HB_Mono10_Packed,\
PixelType_Gvsp_HB_Mono12,\
PixelType_Gvsp_HB_Mono12_Packed,\
PixelType_Gvsp_HB_Mono16,\
PixelType_Gvsp_HB_RGB8_Packed,\
PixelType_Gvsp_HB_BGR8_Packed,\
PixelType_Gvsp_HB_RGBA8_Packed,\
PixelType_Gvsp_HB_BGRA8_Packed,\
PixelType_Gvsp_HB_RGB16_Packed,\
PixelType_Gvsp_HB_BGR16_Packed,\
PixelType_Gvsp_HB_RGBA16_Packed,\
PixelType_Gvsp_HB_BGRA16_Packed,\
PixelType_Gvsp_HB_YUV422_Packed,\
PixelType_Gvsp_HB_YUV422_YUYV_Packed,\
PixelType_Gvsp_HB_BayerGR8,\
PixelType_Gvsp_HB_BayerRG8,\
PixelType_Gvsp_HB_BayerGB8,\
PixelType_Gvsp_HB_BayerBG8,\
PixelType_Gvsp_HB_BayerRBGG8,\
PixelType_Gvsp_HB_BayerGB10,\
PixelType_Gvsp_HB_BayerGB10_Packed,\
PixelType_Gvsp_HB_BayerBG10,\
PixelType_Gvsp_HB_BayerBG10_Packed,\
PixelType_Gvsp_HB_BayerRG10,\
PixelType_Gvsp_HB_BayerRG10_Packed,\
PixelType_Gvsp_HB_BayerGR10,\
PixelType_Gvsp_HB_BayerGR10_Packed,\
PixelType_Gvsp_HB_BayerGB12,\
PixelType_Gvsp_HB_BayerGB12_Packed,\
PixelType_Gvsp_HB_BayerBG12,\
PixelType_Gvsp_HB_BayerBG12_Packed,\
PixelType_Gvsp_HB_BayerRG12,\
PixelType_Gvsp_HB_BayerRG12_Packed,\
PixelType_Gvsp_HB_BayerGR12,\
PixelType_Gvsp_HB_BayerGR12_Packed):
return True
else:
return False
def IsMonoPixelFormat(enPixelType = 0):
if enPixelType in (PixelType_Gvsp_Mono8, \
PixelType_Gvsp_Mono10, \
PixelType_Gvsp_Mono10_Packed, \
PixelType_Gvsp_Mono12, \
PixelType_Gvsp_Mono12_Packed, \
PixelType_Gvsp_Mono14, \
PixelType_Gvsp_Mono16):
return True
else:
return False
#执行1次
g_SaveImageOnce = False
def image_callback2(pstFrame, pUser, bAutoFree):
start_time = cv2.getTickCount()
stFrame = cast(pstFrame, POINTER(MV_FRAME_OUT)).contents
User = ctypes.cast(pUser, ctypes.py_object).value
if stFrame:
# print ("get one frame: Width[%d], Height[%d], nFrameNum[%d] len[%d] nLostPacket[%d]" % (stFrame.stFrameInfo.nWidth, stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nFrameNum ,stFrame.stFrameInfo.nFrameLen, stFrame.stFrameInfo.nLostPacket))
global g_SaveImageOnce
while True:
stDecodeParam = MV_CC_HB_DECODE_PARAM()
stConvertParam = MV_CC_PIXEL_CONVERT_PARAM_EX()
memset(byref(stConvertParam), 0, sizeof(stConvertParam))
# 如果是HB图像，需要进行HB解码
result = IsHBPixelFormat(stFrame.stFrameInfo.enPixelType)
if True == result :
DecodeBufferlen = stFrame.stFrameInfo.nWidth * stFrame.stFrameInfo.nHeight * 3
DecodeBuffer = (c_ubyte * DecodeBufferlen)()
stDecodeParam.pSrcBuf = stFrame.pBufAddr
stDecodeParam.nSrcLen = stFrame.stFrameInfo.nFrameLen
stDecodeParam.pDstBuf = DecodeBuffer
stDecodeParam.nDstBufSize = DecodeBufferlen
ret = User.MV_CC_HBDecode(stDecodeParam)
if ret != 0:
print("HB Decode fail! ret[0x%x]" % ret)
break
stConvertParam.pSrcData = stDecodeParam.pDstBuf
stConvertParam.nSrcDataLen = stDecodeParam.nDstBufLen
stConvertParam.enSrcPixelType = stDecodeParam.enDstPixelType
else:
stConvertParam.pSrcData = stFrame.pBufAddr
stConvertParam.nSrcDataLen = stFrame.stFrameInfo.nFrameLen
stConvertParam.enSrcPixelType = stFrame.stFrameInfo.enPixelType
# 图像进行格式转换
enDstPixelType = PixelType_Gvsp_Undefined
nChannelNum = 1
bMono = IsMonoPixelFormat(stConvertParam.enSrcPixelType)
if True == bMono :
enDstPixelType = PixelType_Gvsp_Mono8
nChannelNum = 1
else:
enDstPixelType = PixelType_Gvsp_BGR8_Packed
nChannelNum = 3
convertdestBufflen = nChannelNum *stFrame.stFrameInfo.nWidth * stFrame.stFrameInfo.nHeight
DstBuffer = (c_ubyte * convertdestBufflen)()
DstBufferlen = convertdestBufflen
stConvertParam.nWidth = stFrame.stFrameInfo.nWidth
stConvertParam.nHeight = stFrame.stFrameInfo.nHeight
stConvertParam.enDstPixelType = enDstPixelType
stConvertParam.pDstBuffer = DstBuffer
stConvertParam.nDstBufferSize = DstBufferlen
ret = User.MV_CC_ConvertPixelTypeEx(stConvertParam)
if ret != 0:
print ("convert pixel fail! ret[0x%x]" % ret)
break
if nChannelNum == 1:
numpy_image = numpy.frombuffer(DstBuffer, dtype=numpy.ubyte, count=DstBufferlen). \
reshape(stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth)
# 执行1次, 只保存1张图
if g_SaveImageOnce == False:
output_path = "output_mono_image.bmp"
cv2.imwrite(output_path, numpy_image)
print("save output_mono_image.bmp success")
g_SaveImageOnce = True
else:
numpy_image = numpy.frombuffer(DstBuffer, dtype=numpy.ubyte, count=DstBufferlen). \
reshape(stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3)
#分辨率缩小一半，提高处理速度
numpy_image = cv2.resize(numpy_image, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA)
# 1. 预处理：灰度图 + 高斯模糊 (去噪)
Gray_image = cv2.cvtColor(numpy_image, cv2.COLOR_BGR2GRAY)
# 稍微增大模糊核，例如 (5,5)，以平滑更多纹理细节
Gauss_image = cv2.GaussianBlur(Gray_image, (5, 5), 1)
# 2. 二值化替代 Canny (更稳健)
# 方法A: Otsu自动阈值 (适合对比度明显的场景)
ret, binary = cv2.threshold(Gauss_image, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
# 方法B: 如果光照不均，建议取消下面注释改用自适应阈值
# binary = cv2.adaptiveThreshold(Gauss_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
# cv2.THRESH_BINARY, 11, 2)
# 3. 形态学操作 (关键步骤：连接断裂，去除噪点)
kernel = numpy.ones((3,3),numpy.uint8) # 核越大，连接能力越强，但也可能使物体变形
dilated_binary = cv2.dilate(binary, kernel, iterations=1) # 膨胀
eroded_binary = cv2.erode(dilated_binary, kernel, iterations=1) # 腐蚀 (先膨胀后腐蚀=闭运算，用于填补空洞)
# 4. 查找轮廓
# 注意：二值化图像中，物体应为白色(255)，背景为黑色(0)
contours, _ = cv2.findContours(eroded_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
for cnt in contours:
cv2.drawContours(numpy_image, [cnt], -1, (0, 255, 0), 1) # 绘制原始轮廓，调试用
# --- 筛选条件 1: 面积过滤 ---
# 忽略太小的噪点，根据实际情况调整数值 (例如忽略小于画面 1% 的区域)
area = cv2.contourArea(cnt)
if area < threshold_area:
continue
# --- 筛选条件 2: 形状近似,必须是四边形且为凸多边形 ---
epsilon = 0.02 * cv2.arcLength(cnt, True)
approx = cv2.approxPolyDP(cnt, epsilon, True)
if len(approx) == 4 and cv2.isContourConvex(approx):
# 绘制结果
cv2.drawContours(numpy_image, [approx], -1, (0, 0, 255), 2)
# 绘制顶点
for point in approx:
cv2.circle(numpy_image, tuple(point[0]), 5, (255, 0, 0), -1)
# fps计算
current_time = cv2.getTickCount()
fps = cv2.getTickFrequency() / (current_time - start_time)
text = f"FPS: {fps:.2f}"
# 在左上角绘制文字
cv2.putText(numpy_image, text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
# 显示结果
cv2.imshow('Result', numpy_image)
cv2.imshow('Binary', eroded_binary) # 调试时可查看二值化效果
# cv2.imshow("ROI", img_ROI)
cv2.waitKey(1)
# 执行1次, 只保存1张图
# if g_SaveImageOnce == False:
# output_path = "output_color_image.bmp"
# cv2.imwrite(output_path, numpy_image)
# print("save output_color_image.bmp success")
# g_SaveImageOnce = True
#正常退出
break
#非自动释放模式，需要额外进行 手动释放资源
if bAutoFree == False :
if pUser != None:
User.MV_CC_FreeImageBuffer(stFrame)
else :
print ("user is null, invalid")
CALL_BACK_FUN2 = FrameInfoCallBack2(image_callback2)
if __name__ == "__main__":
# ch:初始化SDK | en: initialize SDK
MvCamera.MV_CC_Initialize()
deviceList = MV_CC_DEVICE_INFO_LIST()
# ch:选择枚举的传输层协议 (根据需求和系统支持情况选择) | en: Select the Transport Layer Protocol (based on your requirements and system support).
tlayerType = (MV_GIGE_DEVICE | MV_USB_DEVICE | MV_GENTL_CAMERALINK_DEVICE
| MV_GENTL_CXP_DEVICE | MV_GENTL_XOF_DEVICE)
# ch:枚举设备 | en:Enum device
ret = MvCamera.MV_CC_EnumDevices(tlayerType, deviceList)
if ret != 0:
print ("enum devices fail! ret[0x%x]" % ret)
sys.exit()
if deviceList.nDeviceNum == 0:
print ("find no device!")
sys.exit()
print ("find %d devices!" % deviceList.nDeviceNum)
for i in range(0, deviceList.nDeviceNum):
mvcc_dev_info = cast(deviceList.pDeviceInfo[i], POINTER(MV_CC_DEVICE_INFO)).contents
if mvcc_dev_info.nTLayerType == MV_GIGE_DEVICE or mvcc_dev_info.nTLayerType == MV_GENTL_GIGE_DEVICE:
print ("\ngige device: [%d]" % i)
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stGigEInfo.chModelName if c != 0])
print ("device model name: %s" % strModeName)
nip1 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24)
nip2 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16)
nip3 = ((mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8)
nip4 = (mvcc_dev_info.SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff)
print ("current ip: %d.%d.%d.%d\n" % (nip1, nip2, nip3, nip4))
elif mvcc_dev_info.nTLayerType == MV_USB_DEVICE:
print ("\nu3v device: [%d]" % i)
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chModelName if c != 0])
print ("device model name: %s" % strModeName)
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stUsb3VInfo.chSerialNumber if c != 0])
print ("user serial number: %s" % strSerialNumber)
elif mvcc_dev_info.nTLayerType == MV_GENTL_CAMERALINK_DEVICE:
print ("\nCML device: [%d]" % i)
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCMLInfo.chModelName if c != 0])
print ("device model name: %s" % strModeName)
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCMLInfo.chSerialNumber if c != 0])
print ("user serial number: %s" % strSerialNumber)
elif mvcc_dev_info.nTLayerType == MV_GENTL_CXP_DEVICE:
print ("\nCXP device: [%d]" % i)
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCXPInfo.chModelName if c != 0])
print ("device model name: %s" % strModeName)
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stCXPInfo.chSerialNumber if c != 0])
print ("user serial number: %s" % strSerialNumber)
elif mvcc_dev_info.nTLayerType == MV_GENTL_XOF_DEVICE:
print ("\nXoF device: [%d]" % i)
strModeName = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stXoFInfo.chModelName if c != 0])
print ("device model name: %s" % strModeName)
strSerialNumber = ''.join([chr(c) for c in mvcc_dev_info.SpecialInfo.stXoFInfo.chSerialNumber if c != 0])
print ("user serial number: %s" % strSerialNumber)
# nConnectionNum = input_func("please input_func the number of the device to connect:")
nConnectionNum = 0
if int(nConnectionNum) >= deviceList.nDeviceNum:
print ("intput error!")
sys.exit()
# ch:创建相机实例 | en:Creat Camera Object
cam = MvCamera()
# ch:选择设备并创建句柄 | en:Select device and create handle
stDeviceList = cast(deviceList.pDeviceInfo[int(nConnectionNum)], POINTER(MV_CC_DEVICE_INFO)).contents
ret = cam.MV_CC_CreateHandle(stDeviceList)
if ret != 0:
print ("create handle fail! ret[0x%x]" % ret)
sys.exit()
# ch:打开设备 | en:Open device
ret = cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0)
if ret != 0:
print ("open device fail! ret[0x%x]" % ret)
sys.exit()
# ch:探测网络最佳包大小(只对GigE相机有效) | en:Detection network optimal package size(It only works for the GigE camera)
if stDeviceList.nTLayerType == MV_GIGE_DEVICE or stDeviceList.nTLayerType == MV_GENTL_GIGE_DEVICE:
nPacketSize = cam.MV_CC_GetOptimalPacketSize()
if int(nPacketSize) > 0:
ret = cam.MV_CC_SetIntValue("GevSCPSPacketSize",nPacketSize)
if ret != 0:
print ("Warning: Set Packet Size fail! ret[0x%x]" % ret)
else:
print ("Warning: Get Packet Size fail! ret[0x%x]" % nPacketSize)
# ch:设置触发模式为off | en:Set trigger mode as off
ret = cam.MV_CC_SetEnumValue("TriggerMode", MV_TRIGGER_MODE_OFF)
if ret != 0:
print ("set trigger mode fail! ret[0x%x]" % ret)
sys.exit()
# ch:注册抓图回调 | en:Register image callback
ret = cam.MV_CC_RegisterImageCallBackEx2(CALL_BACK_FUN2, ctypes.py_object(cam), True)
if ret != 0:
print ("register image callback fail! ret[0x%x]" % ret)
sys.exit()
# ch:开始取流 | en:Start grab image
ret = cam.MV_CC_StartGrabbing()
if ret != 0:
print ("start grabbing fail! ret[0x%x]" % ret)
sys.exit()
print ("press Enter key to stop grabbing.")
input_func()
cv2.destroyAllWindows()
# ch:停止取流 | en:Stop grab image
ret = cam.MV_CC_StopGrabbing()
if ret != 0:
print ("stop grabbing fail! ret[0x%x]" % ret)
sys.exit()
# ch:关闭设备 | Close device
ret = cam.MV_CC_CloseDevice()
if ret != 0:
print ("close deivce fail! ret[0x%x]" % ret)
sys.exit()
# ch:销毁句柄 | Destroy handle
ret = cam.MV_CC_DestroyHandle()
if ret != 0:
print ("destroy handle fail! ret[0x%x]" % ret)
sys.exit()
# ch:反初始化SDK | en: finalize SDK
MvCamera.MV_CC_Finalize()
workspace/src/algorithm_pkg/algorithm_pkg/__init__.py
Executable
View File
workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py
Executable
+156
View File
@@ -0,0 +1,156 @@
#!/usr/bin/env python3
import numpy as np
import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
from cv_bridge import CvBridge
# 配置项（与 vision_example.py 保持一致）
IMAGE_AREA = 1080 * 1440 / 4
THRESHOLD_AREA = IMAGE_AREA * 0.01
class VisionNode(Node):
def __init__(self):
super().__init__('vision_node')
self._bridge = CvBridge()
self._subscription = self.create_subscription(
Image, '/camera/image_raw', self._image_callback, 10
)
self._result_publisher = self.create_publisher(
Image, '/vision/result_image', 10
)
self._binary_publisher = self.create_publisher(
Image, '/vision/binary_image', 10
)
self._target_publisher = self.create_publisher(
Point, '/vision/target', 10
)
self.get_logger().info(
'VisionNode ready - subscribing to /camera/image_raw'
)
self.start_time = 0
def _image_callback(self, msg):
try:
encoding = msg.encoding
if encoding not in ('bgr8', 'rgb8', 'mono8'):
self.get_logger().warn(f'Unexpected encoding: {encoding}')
return
cv_image = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
# 分辨率缩小一半，提高处理速度
scaled = cv2.resize(cv_image, None, fx=0.5, fy=0.5,
interpolation=cv2.INTER_AREA)
# 预处理：灰度图 + 高斯模糊 (去噪)
gray_image = cv2.cvtColor(scaled, cv2.COLOR_BGR2GRAY)
gauss_image = cv2.GaussianBlur(gray_image, (5, 5), 1)
# Otsu 二值化（替代 Canny，更稳健）
_, binary = cv2.threshold(
gauss_image, 0, 255,
cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU
)
# 形态学闭运算：先膨胀后腐蚀，连接断裂区域、去除噪点
kernel = np.ones((3, 3), np.uint8)
dilated = cv2.dilate(binary, kernel, iterations=1)
eroded = cv2.erode(dilated, kernel, iterations=1)
# 查找轮廓
contours, _ = cv2.findContours(
eroded, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
)
result_image = cv_image.copy()
best_target = None
max_area = 0
for cnt in contours:
# 绘制原始轮廓，调试用
cnt_scaled = (cnt / 0.5).astype(np.int32)
cv2.drawContours(result_image, [cnt_scaled], -1, (0, 255, 0), 1)
# 面积过滤
area = cv2.contourArea(cnt)
if area < THRESHOLD_AREA:
continue
# 形状近似：四边形 + 凸多边形
epsilon = 0.02 * cv2.arcLength(cnt, True)
approx = cv2.approxPolyDP(cnt, epsilon, True)
if len(approx) == 4 and cv2.isContourConvex(approx):
# 坐标映射回原始分辨率
approx_scaled = (approx / 0.5).astype(np.int32)
cv2.drawContours(result_image, [approx_scaled], -1,
(0, 0, 255), 2)
for point in approx_scaled:
cv2.circle(result_image, tuple(point[0]), 5,
(255, 0, 0), -1)
if area > max_area:
max_area = area
best_target = approx
if best_target is not None:
moments = cv2.moments(best_target)
if moments['m00'] > 0:
cx = moments['m10'] / moments['m00']
cy = moments['m01'] / moments['m00']
h, w = scaled.shape[:2]
error_x = (cx - w / 2) / (w / 2)
error_y = (cy - h / 2) / (h / 2)
target_msg = Point()
target_msg.x = error_x
target_msg.y = error_y
target_msg.z = 0.0
self._target_publisher.publish(target_msg)
# FPS 叠加
fps = cv2.getTickFrequency() / (cv2.getTickCount() - self.start_time)
cv2.putText(
result_image, f'FPS: {fps:.2f}',
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2
)
self.start_time = cv2.getTickCount()
# 发布结果图像（带轮廓标注）
result_msg = self._bridge.cv2_to_imgmsg(
result_image, encoding='bgr8'
)
result_msg.header.stamp = msg.header.stamp
result_msg.header.frame_id = msg.header.frame_id
self._result_publisher.publish(result_msg)
# 发布二值化图像（调试用）
binary_color = cv2.cvtColor(eroded, cv2.COLOR_GRAY2BGR)
binary_msg = self._bridge.cv2_to_imgmsg(
binary_color, encoding='bgr8'
)
binary_msg.header.stamp = msg.header.stamp
binary_msg.header.frame_id = msg.header.frame_id
self._binary_publisher.publish(binary_msg)
except Exception as e:
self.get_logger().error(f'Vision processing error: {e}')
def main():
rclpy.init()
node = VisionNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
workspace/src/algorithm_pkg/package.xml
Executable
+23
View File
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>algorithm_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="dev01@todo.todo">dev01</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>geometry_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
workspace/src/algorithm_pkg/resource/algorithm_pkg
Executable
View File
workspace/src/algorithm_pkg/setup.cfg
Executable
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/algorithm_pkg
[install]
install_scripts=$base/lib/algorithm_pkg
workspace/src/algorithm_pkg/setup.py
Executable
+30
View File
@@ -0,0 +1,30 @@
from setuptools import find_packages, setup
package_name = 'algorithm_pkg'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='dev01',
maintainer_email='dev01@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
'vision_node = algorithm_pkg.vision_node:main'
],
},
)
workspace/src/algorithm_pkg/test/test_copyright.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
workspace/src/algorithm_pkg/test/test_flake8.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
workspace/src/algorithm_pkg/test/test_pep257.py
Executable
+23
View File
@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
workspace/src/application_pkg/application_pkg/__init__.py
Executable
View File
workspace/src/application_pkg/application_pkg/web_node.py
Executable
+221
View File
@@ -0,0 +1,221 @@
#!/usr/bin/env python3
import json
import socket
import select
import socketserver
import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge
from http.server import HTTPServer, BaseHTTPRequestHandler
_latest_jpeg = None
_HTML_PAGE = """<!DOCTYPE html>
<html lang="zh">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>Vision Pipeline</title>
<style>
body { font-family: 'Segoe UI', sans-serif; margin: 0; padding: 20px;
background: #0d1117; color: #c9d1d9; }
h1 { text-align: center; color: #58a6ff; font-size: 1.5em; margin-bottom: 20px; }
.container { display: flex; flex-wrap: wrap; gap: 20px; justify-content: center; }
.panel { background: #161b22; border: 1px solid #30363d; border-radius: 8px;
padding: 12px; text-align: center; }
.panel h2 { margin: 0 0 10px 0; font-size: 1em; color: #8b949e; }
.panel img { max-width: 45vw; max-height: 50vh; border-radius: 4px; display: block; }
#status { text-align: center; color: #3fb950; font-size: 0.9em; margin-top: 12px; }
</style>
</head>
<body>
<h1>Vision Pipeline Results</h1>
<div class="container">
<div class="panel">
<h2>Result Image (contours)</h2>
<img id="result" src="/result" alt="Result">
</div>
<div class="panel">
<h2>Binary (Otsu + Close)</h2>
<img id="binary" src="/binary" alt="Binary">
</div>
</div>
<div id="status">Live</div>
<script>
const ts = () => '?t=' + Date.now();
setInterval(() => { document.getElementById('result').src = '/result' + ts(); }, 150);
setInterval(() => { document.getElementById('binary').src = '/binary' + ts(); }, 150);
let lastChange = Date.now();
const observer = new MutationObserver(() => { lastChange = Date.now(); });
observer.observe(document.getElementById('result'), { attributes: true });
setInterval(() => {
const elapsed = (Date.now() - lastChange) / 1000;
const el = document.getElementById('status');
if (elapsed > 3) {
el.textContent = 'No new frames (' + elapsed.toFixed(0) + 's)';
el.style.color = '#f85149';
} else {
el.textContent = 'Live';
el.style.color = '#3fb950';
}
}, 1000);
</script>
</body>
</html>"""
_mode_publisher = None
class _ImageHandler(BaseHTTPRequestHandler):
def do_GET(self):
path = self.path.split('?')[0]
if path == '/' or path == '/index.html':
self.send_response(200)
self.send_header('Content-type', 'text/html; charset=utf-8')
self.end_headers()
self.wfile.write(_HTML_PAGE.encode())
elif path in ('/result', '/binary'):
self._serve_image(path.lstrip('/'))
else:
self.send_response(404)
self.end_headers()
def do_POST(self):
path = self.path.split('?')[0]
if path == '/api/mode/switch':
content_length = int(self.headers.get('Content-Length', 0))
body = self.rfile.read(content_length) if content_length > 0 else b'{}'
try:
data = json.loads(body)
mode = data.get('mode', '')
except json.JSONDecodeError:
self.send_response(400)
self.end_headers()
self.wfile.write(b'{"error":"invalid json"}')
return
valid_modes = ('auto', 'manual', 'debug', 'laser_on', 'laser_off')
if mode not in valid_modes:
self.send_response(400)
self.end_headers()
self.wfile.write(
f'{{"error":"invalid mode, must be one of {valid_modes}"}}'
.encode()
)
return
if _mode_publisher is not None:
msg = String()
msg.data = mode
_mode_publisher.publish(msg)
self.send_response(200)
self.send_header('Content-type', 'application/json')
self.end_headers()
self.wfile.write(f'{{"status":"ok","mode":"{mode}"}}'.encode())
else:
self.send_response(404)
self.end_headers()
def _serve_image(self, key):
global _latest_jpeg
jpeg = _latest_jpeg.get(key) if _latest_jpeg else None
if jpeg is None:
self.send_response(204)
self.end_headers()
return
self.send_response(200)
self.send_header('Content-type', 'image/jpeg')
self.send_header('Cache-Control', 'no-cache, no-store, must-revalidate')
self.end_headers()
try:
self.wfile.write(jpeg)
except (BrokenPipeError, ConnectionResetError):
pass
def handle_error(self, request, client_address):
pass
def log_message(self, format, *args):
pass
class _ThreadingHTTPServer(socketserver.ThreadingMixIn, HTTPServer):
daemon_threads = True
class WebNode(Node):
def __init__(self, port=8080):
super().__init__('web_node')
global _mode_publisher
self._bridge = CvBridge()
self._result_sub = self.create_subscription(
Image, '/vision/result_image', self._make_jpeg_callback('result'), 10
)
self._binary_sub = self.create_subscription(
Image, '/vision/binary_image', self._make_jpeg_callback('binary'), 10
)
self._mode_publisher = self.create_publisher(
String, '/gimbal/mode', 10
)
_mode_publisher = self._mode_publisher
self._server = _ThreadingHTTPServer(('', port), _ImageHandler)
self._server.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._server.socket.setblocking(False)
self._server.timeout = 0
# 定时器在主线程中轮询 accept，连接交给工作线程处理
self._timer = self.create_timer(0.01, self._handle_http_requests)
hostname = socket.gethostname()
ip = socket.gethostbyname(hostname)
self.get_logger().info(
f'Web server on http://{ip}:{port} (http://localhost:{port})'
)
def _handle_http_requests(self):
# 一次 tick 内处理所有积压连接
for _ in range(20):
try:
self._server.handle_request()
except Exception:
break
def _make_jpeg_callback(self, key):
def callback(msg):
global _latest_jpeg
try:
cv_image = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
_, jpeg = cv2.imencode('.jpg', cv_image,
[cv2.IMWRITE_JPEG_QUALITY, 85])
if _latest_jpeg is None:
_latest_jpeg = {}
_latest_jpeg[key] = jpeg.tobytes()
except Exception as e:
self.get_logger().error(f'JPEG encode error ({key}): {e}')
return callback
def destroy_node(self):
self._server.shutdown()
super().destroy_node()
def main():
rclpy.init()
node = WebNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
workspace/src/application_pkg/package.xml
Executable
+23
View File
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>application_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="dev01@todo.todo">dev01</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>std_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
workspace/src/application_pkg/resource/application_pkg
Executable
View File
workspace/src/application_pkg/setup.cfg
Executable
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/application_pkg
[install]
install_scripts=$base/lib/application_pkg
workspace/src/application_pkg/setup.py
Executable
+30
View File
@@ -0,0 +1,30 @@
from setuptools import find_packages, setup
package_name = 'application_pkg'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='dev01',
maintainer_email='dev01@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
'web_node = application_pkg.web_node:main'
],
},
)
workspace/src/application_pkg/test/test_copyright.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
workspace/src/application_pkg/test/test_flake8.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
workspace/src/application_pkg/test/test_pep257.py
Executable
+23
View File
@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
workspace/src/bringup_pkg/bringup_pkg/__init__.py
Executable
View File
workspace/src/bringup_pkg/launch/vision_pipeline.launch.py
Executable
+35
View File
@@ -0,0 +1,35 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='driver_pkg',
executable='camera_node',
name='camera_node',
output='screen',
),
Node(
package='algorithm_pkg',
executable='vision_node',
name='vision_node',
output='screen',
),
Node(
package='application_pkg',
executable='web_node',
name='web_node',
output='screen',
),
Node(
package='gimbal_control_pkg',
executable='gimbal_node',
name='gimbal_node',
output='screen',
parameters=[{
'serial_port': '/dev/ttyTHS0',
'baud_rate': 115200,
}],
),
])
workspace/src/bringup_pkg/package.xml
Executable
+22
View File
@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>bringup_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="dev01@todo.todo">dev01</maintainer>
<license>TODO: License declaration</license>
<depend>driver_pkg</depend>
<depend>algorithm_pkg</depend>
<depend>application_pkg</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
workspace/src/bringup_pkg/resource/bringup_pkg
Executable
View File
workspace/src/bringup_pkg/setup.cfg
Executable
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/bringup_pkg
[install]
install_scripts=$base/lib/bringup_pkg
workspace/src/bringup_pkg/setup.py
Executable
+33
View File
@@ -0,0 +1,33 @@
import os
from glob import glob
from setuptools import find_packages, setup
package_name = 'bringup_pkg'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'),
glob('launch/*.launch.py')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='dev01',
maintainer_email='dev01@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
],
},
)
workspace/src/bringup_pkg/test/test_copyright.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
workspace/src/bringup_pkg/test/test_flake8.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
workspace/src/bringup_pkg/test/test_pep257.py
Executable
+23
View File
@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
workspace/src/driver_pkg/driver_pkg/__init__.py
Executable
View File
workspace/src/driver_pkg/driver_pkg/camera_node.py
Executable
+253
View File
@@ -0,0 +1,253 @@
#!/usr/bin/env python3
import sys
import platform
import os
import ctypes
from ctypes import *
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
_current_system = platform.system()
if _current_system == 'Windows':
sys.path.append(os.getenv('MVCAM_COMMON_RUNENV') + '/Samples/Python/MvImport')
else:
sys.path.append('/opt/MVS/Samples/aarch64/Python/MvImport')
from MvCameraControl_class import *
_stFrame = POINTER(MV_FRAME_OUT)
_fun_ctype = get_platform_functype()
_FrameInfoCallBack2 = _fun_ctype(None, _stFrame, c_void_p, c_bool)
def _is_hb_pixel_format(enPixelType):
return enPixelType in (
PixelType_Gvsp_HB_Mono8,
PixelType_Gvsp_HB_Mono10,
PixelType_Gvsp_HB_Mono10_Packed,
PixelType_Gvsp_HB_Mono12,
PixelType_Gvsp_HB_Mono12_Packed,
PixelType_Gvsp_HB_Mono16,
PixelType_Gvsp_HB_RGB8_Packed,
PixelType_Gvsp_HB_BGR8_Packed,
PixelType_Gvsp_HB_RGBA8_Packed,
PixelType_Gvsp_HB_BGRA8_Packed,
PixelType_Gvsp_HB_RGB16_Packed,
PixelType_Gvsp_HB_BGR16_Packed,
PixelType_Gvsp_HB_RGBA16_Packed,
PixelType_Gvsp_HB_BGRA16_Packed,
PixelType_Gvsp_HB_YUV422_Packed,
PixelType_Gvsp_HB_YUV422_YUYV_Packed,
PixelType_Gvsp_HB_BayerGR8,
PixelType_Gvsp_HB_BayerRG8,
PixelType_Gvsp_HB_BayerGB8,
PixelType_Gvsp_HB_BayerBG8,
PixelType_Gvsp_HB_BayerRBGG8,
PixelType_Gvsp_HB_BayerGB10,
PixelType_Gvsp_HB_BayerGB10_Packed,
PixelType_Gvsp_HB_BayerBG10,
PixelType_Gvsp_HB_BayerBG10_Packed,
PixelType_Gvsp_HB_BayerRG10,
PixelType_Gvsp_HB_BayerRG10_Packed,
PixelType_Gvsp_HB_BayerGR10,
PixelType_Gvsp_HB_BayerGR10_Packed,
PixelType_Gvsp_HB_BayerGB12,
PixelType_Gvsp_HB_BayerGB12_Packed,
PixelType_Gvsp_HB_BayerBG12,
PixelType_Gvsp_HB_BayerBG12_Packed,
PixelType_Gvsp_HB_BayerRG12,
PixelType_Gvsp_HB_BayerRG12_Packed,
PixelType_Gvsp_HB_BayerGR12,
PixelType_Gvsp_HB_BayerGR12_Packed,
)
def _is_mono_pixel_format(enPixelType):
return enPixelType in (
PixelType_Gvsp_Mono8,
PixelType_Gvsp_Mono10,
PixelType_Gvsp_Mono10_Packed,
PixelType_Gvsp_Mono12,
PixelType_Gvsp_Mono12_Packed,
PixelType_Gvsp_Mono14,
PixelType_Gvsp_Mono16,
)
class CameraNode(Node):
def __init__(self):
super().__init__('camera_node')
self._bridge = CvBridge()
self._publisher = self.create_publisher(Image, '/camera/image_raw', 10)
self._cam = None
self.get_logger().info('Initializing MvCamera SDK...')
self._init_camera()
self.get_logger().info('Camera started, publishing to /camera/image_raw')
def _init_camera(self):
MvCamera.MV_CC_Initialize()
deviceList = MV_CC_DEVICE_INFO_LIST()
tlayerType = (
MV_GIGE_DEVICE
| MV_USB_DEVICE
| MV_GENTL_CAMERALINK_DEVICE
| MV_GENTL_CXP_DEVICE
| MV_GENTL_XOF_DEVICE
)
ret = MvCamera.MV_CC_EnumDevices(tlayerType, deviceList)
if ret != 0:
self.get_logger().error(f'Enum devices fail! ret[0x{ret:x}]')
raise RuntimeError('Failed to enumerate devices')
if deviceList.nDeviceNum == 0:
raise RuntimeError('No camera device found')
self.get_logger().info(f'Found {deviceList.nDeviceNum} device(s)')
stDeviceList = cast(
deviceList.pDeviceInfo[0], POINTER(MV_CC_DEVICE_INFO)
).contents
self._cam = MvCamera()
ret = self._cam.MV_CC_CreateHandle(stDeviceList)
if ret != 0:
raise RuntimeError(f'Create handle fail! ret[0x{ret:x}]')
ret = self._cam.MV_CC_OpenDevice(MV_ACCESS_Exclusive, 0)
if ret != 0:
raise RuntimeError(f'Open device fail! ret[0x{ret:x}]')
if stDeviceList.nTLayerType in (MV_GIGE_DEVICE, MV_GENTL_GIGE_DEVICE):
nPacketSize = self._cam.MV_CC_GetOptimalPacketSize()
if nPacketSize > 0:
self._cam.MV_CC_SetIntValue('GevSCPSPacketSize', nPacketSize)
ret = self._cam.MV_CC_SetEnumValue('TriggerMode', MV_TRIGGER_MODE_OFF)
if ret != 0:
raise RuntimeError(f'Set trigger mode fail! ret[0x{ret:x}]')
self._callback = _FrameInfoCallBack2(self._image_callback)
ret = self._cam.MV_CC_RegisterImageCallBackEx2(
self._callback, ctypes.py_object(self._cam), True
)
if ret != 0:
raise RuntimeError(f'Register image callback fail! ret[0x{ret:x}]')
ret = self._cam.MV_CC_StartGrabbing()
if ret != 0:
raise RuntimeError(f'Start grabbing fail! ret[0x{ret:x}]')
def _image_callback(self, pstFrame, pUser, bAutoFree):
try:
stFrame = cast(pstFrame, POINTER(MV_FRAME_OUT)).contents
cam_ptr = ctypes.cast(pUser, ctypes.py_object).value
if not stFrame:
return
while True:
stDecodeParam = MV_CC_HB_DECODE_PARAM()
stConvertParam = MV_CC_PIXEL_CONVERT_PARAM_EX()
memset(byref(stConvertParam), 0, sizeof(stConvertParam))
if _is_hb_pixel_format(stFrame.stFrameInfo.enPixelType):
decodeBufLen = (
stFrame.stFrameInfo.nWidth
* stFrame.stFrameInfo.nHeight
* 3
)
decodeBuf = (c_ubyte * decodeBufLen)()
stDecodeParam.pSrcBuf = stFrame.pBufAddr
stDecodeParam.nSrcLen = stFrame.stFrameInfo.nFrameLen
stDecodeParam.pDstBuf = decodeBuf
stDecodeParam.nDstBufSize = decodeBufLen
ret = cam_ptr.MV_CC_HBDecode(stDecodeParam)
if ret != 0:
break
stConvertParam.pSrcData = stDecodeParam.pDstBuf
stConvertParam.nSrcDataLen = stDecodeParam.nDstBufLen
stConvertParam.enSrcPixelType = stDecodeParam.enDstPixelType
else:
stConvertParam.pSrcData = stFrame.pBufAddr
stConvertParam.nSrcDataLen = stFrame.stFrameInfo.nFrameLen
stConvertParam.enSrcPixelType = stFrame.stFrameInfo.enPixelType
if _is_mono_pixel_format(stConvertParam.enSrcPixelType):
enDstPixelType = PixelType_Gvsp_Mono8
nChannelNum = 1
encoding = 'mono8'
else:
enDstPixelType = PixelType_Gvsp_BGR8_Packed
nChannelNum = 3
encoding = 'bgr8'
dstBufLen = (
nChannelNum
* stFrame.stFrameInfo.nWidth
* stFrame.stFrameInfo.nHeight
)
dstBuf = (c_ubyte * dstBufLen)()
stConvertParam.nWidth = stFrame.stFrameInfo.nWidth
stConvertParam.nHeight = stFrame.stFrameInfo.nHeight
stConvertParam.enDstPixelType = enDstPixelType
stConvertParam.pDstBuffer = dstBuf
stConvertParam.nDstBufferSize = dstBufLen
ret = cam_ptr.MV_CC_ConvertPixelTypeEx(stConvertParam)
if ret != 0:
break
if nChannelNum == 1:
numpy_image = np.frombuffer(
dstBuf, dtype=np.ubyte, count=dstBufLen
).reshape(stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth)
else:
numpy_image = np.frombuffer(
dstBuf, dtype=np.ubyte, count=dstBufLen
).reshape(
stFrame.stFrameInfo.nHeight, stFrame.stFrameInfo.nWidth, 3
)
msg = self._bridge.cv2_to_imgmsg(numpy_image, encoding=encoding)
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'camera_frame'
self._publisher.publish(msg)
break
if not bAutoFree and pUser is not None:
cam_ptr.MV_CC_FreeImageBuffer(stFrame)
except Exception as e:
self.get_logger().error(f'Image callback error: {e}')
def cleanup(self):
self.get_logger().info('Stopping camera...')
if self._cam is not None:
self._cam.MV_CC_StopGrabbing()
self._cam.MV_CC_CloseDevice()
self._cam.MV_CC_DestroyHandle()
MvCamera.MV_CC_Finalize()
self.get_logger().info('Camera released')
def main():
rclpy.init()
node = CameraNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.cleanup()
node.destroy_node()
rclpy.shutdown()
workspace/src/driver_pkg/package.xml
Executable
+22
View File
@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>driver_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="dev01@todo.todo">dev01</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
workspace/src/driver_pkg/resource/driver_pkg
Executable
View File
workspace/src/driver_pkg/setup.cfg
Executable
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/driver_pkg
[install]
install_scripts=$base/lib/driver_pkg
workspace/src/driver_pkg/setup.py
Executable
+30
View File
@@ -0,0 +1,30 @@
from setuptools import find_packages, setup
package_name = 'driver_pkg'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='dev01',
maintainer_email='dev01@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
'camera_node = driver_pkg.camera_node:main'
],
},
)
workspace/src/driver_pkg/test/test_copyright.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
workspace/src/driver_pkg/test/test_flake8.py
Executable
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
workspace/src/driver_pkg/test/test_pep257.py
Executable
+23
View File
@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
workspace/src/gimbal_control_pkg/gimbal_control_pkg/__init__.py
View File
workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
+162
View File
@@ -0,0 +1,162 @@
#!/usr/bin/env python3
import struct
import json
import serial
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, Point
from std_msgs.msg import String
CMD_AUTO = 0x01
CMD_MANUAL = 0x02
CMD_LASER_ON = 0x03
CMD_LASER_OFF = 0x04
CMD_DEBUG = 0x05
def _checksum(data: bytearray) -> int:
return sum(data) & 0xFF
class GimbalNode(Node):
def __init__(self):
super().__init__('gimbal_node')
self.declare_parameter('serial_port', '/dev/ttyTHS0')
self.declare_parameter('baud_rate', 115200)
self.declare_parameter('auto_tracking_enabled', True)
port = self.get_parameter('serial_port').value
baud = self.get_parameter('baud_rate').value
self._ser = serial.Serial(
port=port,
baudrate=baud,
bytesize=serial.EIGHTBITS,
stopbits=serial.STOPBITS_ONE,
parity=serial.PARITY_NONE,
timeout=0.1,
)
self.get_logger().info(f'Serial port {port} opened at {baud} baud')
self._cmd_gimbal_sub = self.create_subscription(
Twist, '/cmd_gimbal', self._cmd_gimbal_callback, 10
)
self._target_sub = self.create_subscription(
Point, '/vision/target', self._target_callback, 10
)
self._pid_config_sub = self.create_subscription(
String, '/pid_config', self._pid_config_callback, 10
)
self._mode_sub = self.create_subscription(
String, '/gimbal/mode', self._mode_callback, 10
)
self.get_logger().info(
'GimbalNode ready - subs: /cmd_gimbal, /vision/target, '
'/pid_config, /gimbal/mode'
)
def _write_frame(self, data: bytearray):
try:
self._ser.write(data)
self._ser.flush()
except serial.SerialException as e:
self.get_logger().error(f'Serial write error: {e}')
def _cmd_gimbal_callback(self, msg: Twist):
yaw_deg = msg.angular.z
pitch_deg = msg.angular.y
self.get_logger().debug(
f'Manual control: yaw={yaw_deg:.2f}, pitch={pitch_deg:.2f}'
)
data = bytearray()
data.append(0xA6)
data.append(0) # zero_force = 0
data.extend(struct.pack('<ff', float(yaw_deg), float(pitch_deg)))
data.append(_checksum(data))
self._write_frame(data)
def _target_callback(self, msg: Point):
self.get_logger().debug(
f'Auto tracking: error_x={msg.x:.4f}, error_y={msg.y:.4f}'
)
data = bytearray()
data.append(0xA5)
data.extend(struct.pack('<ff', float(msg.x), float(msg.y)))
data.append(_checksum(data))
self._write_frame(data)
def _pid_config_callback(self, msg: String):
try:
pid = json.loads(msg.data)
except json.JSONDecodeError as e:
self.get_logger().error(f'Invalid PID JSON: {e}')
return
v_yaw = pid.get('vision_yaw', {})
v_pitch = pid.get('vision_pitch', {})
m_yaw = pid.get('motor_yaw', {})
m_pitch = pid.get('motor_pitch', {})
self.get_logger().info('Sending PID debug tuning frame')
data = bytearray()
data.append(0xA8)
data.extend(struct.pack(
'<fff', v_yaw.get('kp', 0.0), v_yaw.get('ki', 0.0),
v_yaw.get('kd', 0.0)
))
data.extend(struct.pack(
'<fff', v_pitch.get('kp', 0.0), v_pitch.get('ki', 0.0),
v_pitch.get('kd', 0.0)
))
data.extend(struct.pack(
'<fff', m_yaw.get('kp', 0.0), m_yaw.get('ki', 0.0),
m_yaw.get('kd', 0.0)
))
data.extend(struct.pack(
'<fff', m_pitch.get('kp', 0.0), m_pitch.get('ki', 0.0),
m_pitch.get('kd', 0.0)
))
data.append(_checksum(data))
self._write_frame(data)
def _mode_callback(self, msg: String):
mode = msg.data.strip().lower()
if mode == 'auto':
cmd = CMD_AUTO
elif mode == 'manual':
cmd = CMD_MANUAL
elif mode == 'debug':
cmd = CMD_DEBUG
elif mode == 'laser_on':
cmd = CMD_LASER_ON
elif mode == 'laser_off':
cmd = CMD_LASER_OFF
else:
self.get_logger().warn(f'Unknown mode command: {mode}')
return
self.get_logger().info(f'Sending command: 0xA7 cmd=0x{cmd:02X} ({mode})')
data = bytearray([0xA7, cmd])
data.append(_checksum(data))
self._write_frame(data)
def destroy_node(self):
if self._ser and self._ser.is_open:
self._ser.close()
self.get_logger().info('Serial port closed')
super().destroy_node()
def main():
rclpy.init()
node = GimbalNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
workspace/src/gimbal_control_pkg/package.xml
+23
View File
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gimbal_control_pkg</name>
<version>0.0.0</version>
<description>Gimbal serial communication node (UART 115200 8N1)</description>
<maintainer email="dev01@todo.todo">dev01</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
workspace/src/gimbal_control_pkg/resource/gimbal_control_pkg
View File
workspace/src/gimbal_control_pkg/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/gimbal_control_pkg
[install]
install_scripts=$base/lib/gimbal_control_pkg
workspace/src/gimbal_control_pkg/setup.py
+28
View File
@@ -0,0 +1,28 @@
from setuptools import find_packages, setup
package_name = 'gimbal_control_pkg'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='dev01',
maintainer_email='dev01@todo.todo',
description='Gimbal serial communication node',
license='TODO: License declaration',
extras_require={
'test': ['pytest'],
},
entry_points={
'console_scripts': [
'gimbal_node = gimbal_control_pkg.gimbal_node:main'
],
},
)
workspace/src/gimbal_control_pkg/test/test_copyright.py
+24
View File
@@ -0,0 +1,24 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
workspace/src/gimbal_control_pkg/test/test_flake8.py
+25
View File
@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
workspace/src/gimbal_control_pkg/test/test_pep257.py
+23
View File
@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
workspace/src/gimbal_control_pkg/test/test_protocol.py
+74
View File
@@ -0,0 +1,74 @@
# Copyright 2024
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import pytest
from gimbal_control_pkg.gimbal_node import _checksum, CMD_AUTO, CMD_MANUAL
from gimbal_control_pkg.gimbal_node import CMD_DEBUG, CMD_LASER_ON, CMD_LASER_OFF
import struct
def test_checksum():
data = bytearray([0xA7, 0x01])
assert _checksum(data) == (0xA7 + 0x01) & 0xFF
def test_checksum_zero():
assert _checksum(bytearray()) == 0
def test_cmd_constants():
assert CMD_AUTO == 0x01
assert CMD_MANUAL == 0x02
assert CMD_LASER_ON == 0x03
assert CMD_LASER_OFF == 0x04
assert CMD_DEBUG == 0x05
def test_auto_tracking_frame():
error_x, error_y = 0.5, -0.3
data = bytearray()
data.append(0xA5)
data.extend(struct.pack('<ff', error_x, error_y))
data.append(_checksum(data))
assert len(data) == 10
assert data[0] == 0xA5
x, y = struct.unpack_from('<ff', data, 1)
assert abs(x - error_x) < 1e-6
assert abs(y - error_y) < 1e-6
def test_manual_control_frame():
yaw, pitch = 30.0, -15.0
data = bytearray()
data.append(0xA6)
data.append(0)
data.extend(struct.pack('<ff', yaw, pitch))
data.append(_checksum(data))
assert len(data) == 11
assert data[0] == 0xA6
def test_command_frame():
data = bytearray([0xA7, CMD_AUTO])
data.append(_checksum(data))
assert len(data) == 3
def test_debug_tune_frame():
data = bytearray()
data.append(0xA8)
for _ in range(12):
data.extend(struct.pack('<f', 1.0))
data.append(_checksum(data))
assert len(data) == 50