commit d506e0118896f12cff4a6ad10ca29f594f7c3ca8 Author: dev01 Date: Sun May 31 22:10:12 2026 +0800 feat: initial ROS2 vision pipeline with gimbal control and nginx config - 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 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..aba86d7 --- /dev/null +++ b/.gitignore @@ -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 \ No newline at end of file diff --git a/config/nginx-aimbot.conf b/config/nginx-aimbot.conf new file mode 100644 index 0000000..a511833 --- /dev/null +++ b/config/nginx-aimbot.conf @@ -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; + } +} diff --git a/docs/与前端对接文档.md b/docs/与前端对接文档.md new file mode 100644 index 0000000..10191f7 --- /dev/null +++ b/docs/与前端对接文档.md @@ -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= +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://: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` | diff --git a/docs/云台通信协议指南.md b/docs/云台通信协议指南.md new file mode 100644 index 0000000..9c1dc30 --- /dev/null +++ b/docs/云台通信协议指南.md @@ -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(' int: + return sum(data) & 0xFF # 所有字节累加取低 8 位 +``` + +校验和覆盖范围：**帧头到数据末尾前一字节**的全部字节。接收端在收到数据后也应计算并比对校验和。 diff --git a/vision_example.py b/vision_example.py new file mode 100755 index 0000000..7ed10d0 --- /dev/null +++ b/vision_example.py @@ -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() diff --git a/workspace/src/algorithm_pkg/algorithm_pkg/__init__.py b/workspace/src/algorithm_pkg/algorithm_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py b/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py new file mode 100755 index 0000000..4c8023a --- /dev/null +++ b/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py @@ -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() diff --git a/workspace/src/algorithm_pkg/package.xml b/workspace/src/algorithm_pkg/package.xml new file mode 100755 index 0000000..cdf3c3d --- /dev/null +++ b/workspace/src/algorithm_pkg/package.xml @@ -0,0 +1,23 @@ + + + + algorithm_pkg + 0.0.0 + TODO: Package description + dev01 + TODO: License declaration + + rclpy + sensor_msgs + cv_bridge + geometry_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/workspace/src/algorithm_pkg/resource/algorithm_pkg b/workspace/src/algorithm_pkg/resource/algorithm_pkg new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/algorithm_pkg/setup.cfg b/workspace/src/algorithm_pkg/setup.cfg new file mode 100755 index 0000000..9300fcc --- /dev/null +++ b/workspace/src/algorithm_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/algorithm_pkg +[install] +install_scripts=$base/lib/algorithm_pkg diff --git a/workspace/src/algorithm_pkg/setup.py b/workspace/src/algorithm_pkg/setup.py new file mode 100755 index 0000000..90e71e4 --- /dev/null +++ b/workspace/src/algorithm_pkg/setup.py @@ -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' + ], + }, +) diff --git a/workspace/src/algorithm_pkg/test/test_copyright.py b/workspace/src/algorithm_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/workspace/src/algorithm_pkg/test/test_copyright.py @@ -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' diff --git a/workspace/src/algorithm_pkg/test/test_flake8.py b/workspace/src/algorithm_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/workspace/src/algorithm_pkg/test/test_flake8.py @@ -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) diff --git a/workspace/src/algorithm_pkg/test/test_pep257.py b/workspace/src/algorithm_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/workspace/src/algorithm_pkg/test/test_pep257.py @@ -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' diff --git a/workspace/src/application_pkg/application_pkg/__init__.py b/workspace/src/application_pkg/application_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/application_pkg/application_pkg/web_node.py b/workspace/src/application_pkg/application_pkg/web_node.py new file mode 100755 index 0000000..2c7ca10 --- /dev/null +++ b/workspace/src/application_pkg/application_pkg/web_node.py @@ -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 = """ + + + + +Vision Pipeline + + + +

Vision Pipeline Results

+
+
+

Result Image (contours)

+ Result +
+
+

Binary (Otsu + Close)

+ Binary +
+
+
Live
+ + +""" + + +_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() diff --git a/workspace/src/application_pkg/package.xml b/workspace/src/application_pkg/package.xml new file mode 100755 index 0000000..0583f12 --- /dev/null +++ b/workspace/src/application_pkg/package.xml @@ -0,0 +1,23 @@ + + + + application_pkg + 0.0.0 + TODO: Package description + dev01 + TODO: License declaration + + rclpy + sensor_msgs + cv_bridge + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/workspace/src/application_pkg/resource/application_pkg b/workspace/src/application_pkg/resource/application_pkg new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/application_pkg/setup.cfg b/workspace/src/application_pkg/setup.cfg new file mode 100755 index 0000000..b26fc08 --- /dev/null +++ b/workspace/src/application_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/application_pkg +[install] +install_scripts=$base/lib/application_pkg diff --git a/workspace/src/application_pkg/setup.py b/workspace/src/application_pkg/setup.py new file mode 100755 index 0000000..587e4a2 --- /dev/null +++ b/workspace/src/application_pkg/setup.py @@ -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' + ], + }, +) diff --git a/workspace/src/application_pkg/test/test_copyright.py b/workspace/src/application_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/workspace/src/application_pkg/test/test_copyright.py @@ -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' diff --git a/workspace/src/application_pkg/test/test_flake8.py b/workspace/src/application_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/workspace/src/application_pkg/test/test_flake8.py @@ -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) diff --git a/workspace/src/application_pkg/test/test_pep257.py b/workspace/src/application_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/workspace/src/application_pkg/test/test_pep257.py @@ -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' diff --git a/workspace/src/bringup_pkg/bringup_pkg/__init__.py b/workspace/src/bringup_pkg/bringup_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/bringup_pkg/launch/vision_pipeline.launch.py b/workspace/src/bringup_pkg/launch/vision_pipeline.launch.py new file mode 100755 index 0000000..18dc3e3 --- /dev/null +++ b/workspace/src/bringup_pkg/launch/vision_pipeline.launch.py @@ -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, + }], + ), + ]) diff --git a/workspace/src/bringup_pkg/package.xml b/workspace/src/bringup_pkg/package.xml new file mode 100755 index 0000000..5708692 --- /dev/null +++ b/workspace/src/bringup_pkg/package.xml @@ -0,0 +1,22 @@ + + + + bringup_pkg + 0.0.0 + TODO: Package description + dev01 + TODO: License declaration + + driver_pkg + algorithm_pkg + application_pkg + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/workspace/src/bringup_pkg/resource/bringup_pkg b/workspace/src/bringup_pkg/resource/bringup_pkg new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/bringup_pkg/setup.cfg b/workspace/src/bringup_pkg/setup.cfg new file mode 100755 index 0000000..5acf639 --- /dev/null +++ b/workspace/src/bringup_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/bringup_pkg +[install] +install_scripts=$base/lib/bringup_pkg diff --git a/workspace/src/bringup_pkg/setup.py b/workspace/src/bringup_pkg/setup.py new file mode 100755 index 0000000..ed32ff0 --- /dev/null +++ b/workspace/src/bringup_pkg/setup.py @@ -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': [ + ], + }, +) diff --git a/workspace/src/bringup_pkg/test/test_copyright.py b/workspace/src/bringup_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/workspace/src/bringup_pkg/test/test_copyright.py @@ -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' diff --git a/workspace/src/bringup_pkg/test/test_flake8.py b/workspace/src/bringup_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/workspace/src/bringup_pkg/test/test_flake8.py @@ -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) diff --git a/workspace/src/bringup_pkg/test/test_pep257.py b/workspace/src/bringup_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/workspace/src/bringup_pkg/test/test_pep257.py @@ -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' diff --git a/workspace/src/driver_pkg/driver_pkg/__init__.py b/workspace/src/driver_pkg/driver_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/driver_pkg/driver_pkg/camera_node.py b/workspace/src/driver_pkg/driver_pkg/camera_node.py new file mode 100755 index 0000000..c36a84b --- /dev/null +++ b/workspace/src/driver_pkg/driver_pkg/camera_node.py @@ -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() diff --git a/workspace/src/driver_pkg/package.xml b/workspace/src/driver_pkg/package.xml new file mode 100755 index 0000000..8f4e865 --- /dev/null +++ b/workspace/src/driver_pkg/package.xml @@ -0,0 +1,22 @@ + + + + driver_pkg + 0.0.0 + TODO: Package description + dev01 + TODO: License declaration + + rclpy + sensor_msgs + cv_bridge + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/workspace/src/driver_pkg/resource/driver_pkg b/workspace/src/driver_pkg/resource/driver_pkg new file mode 100755 index 0000000..e69de29 diff --git a/workspace/src/driver_pkg/setup.cfg b/workspace/src/driver_pkg/setup.cfg new file mode 100755 index 0000000..05cd64a --- /dev/null +++ b/workspace/src/driver_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/driver_pkg +[install] +install_scripts=$base/lib/driver_pkg diff --git a/workspace/src/driver_pkg/setup.py b/workspace/src/driver_pkg/setup.py new file mode 100755 index 0000000..4fff751 --- /dev/null +++ b/workspace/src/driver_pkg/setup.py @@ -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' + ], + }, +) diff --git a/workspace/src/driver_pkg/test/test_copyright.py b/workspace/src/driver_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/workspace/src/driver_pkg/test/test_copyright.py @@ -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' diff --git a/workspace/src/driver_pkg/test/test_flake8.py b/workspace/src/driver_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/workspace/src/driver_pkg/test/test_flake8.py @@ -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) diff --git a/workspace/src/driver_pkg/test/test_pep257.py b/workspace/src/driver_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/workspace/src/driver_pkg/test/test_pep257.py @@ -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' diff --git a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/__init__.py b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py new file mode 100644 index 0000000..e016dbc --- /dev/null +++ b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py @@ -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(' + + + gimbal_control_pkg + 0.0.0 + Gimbal serial communication node (UART 115200 8N1) + dev01 + TODO: License declaration + + rclpy + std_msgs + geometry_msgs + sensor_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/workspace/src/gimbal_control_pkg/resource/gimbal_control_pkg b/workspace/src/gimbal_control_pkg/resource/gimbal_control_pkg new file mode 100644 index 0000000..e69de29 diff --git a/workspace/src/gimbal_control_pkg/setup.cfg b/workspace/src/gimbal_control_pkg/setup.cfg new file mode 100644 index 0000000..d89f2ec --- /dev/null +++ b/workspace/src/gimbal_control_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/gimbal_control_pkg +[install] +install_scripts=$base/lib/gimbal_control_pkg diff --git a/workspace/src/gimbal_control_pkg/setup.py b/workspace/src/gimbal_control_pkg/setup.py new file mode 100644 index 0000000..f6fa76a --- /dev/null +++ b/workspace/src/gimbal_control_pkg/setup.py @@ -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' + ], + }, +) diff --git a/workspace/src/gimbal_control_pkg/test/test_copyright.py b/workspace/src/gimbal_control_pkg/test/test_copyright.py new file mode 100644 index 0000000..b992092 --- /dev/null +++ b/workspace/src/gimbal_control_pkg/test/test_copyright.py @@ -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' diff --git a/workspace/src/gimbal_control_pkg/test/test_flake8.py b/workspace/src/gimbal_control_pkg/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/workspace/src/gimbal_control_pkg/test/test_flake8.py @@ -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) diff --git a/workspace/src/gimbal_control_pkg/test/test_pep257.py b/workspace/src/gimbal_control_pkg/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/workspace/src/gimbal_control_pkg/test/test_pep257.py @@ -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' diff --git a/workspace/src/gimbal_control_pkg/test/test_protocol.py b/workspace/src/gimbal_control_pkg/test/test_protocol.py new file mode 100644 index 0000000..9817239 --- /dev/null +++ b/workspace/src/gimbal_control_pkg/test/test_protocol.py @@ -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('