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
This commit is contained in:
dev01
+156
@@ -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()
|
||||
Executable
+23
@@ -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>
|
||||
Executable
+4
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/algorithm_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/algorithm_pkg
|
||||
Executable
+30
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
+25
@@ -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'
|
||||
+25
@@ -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)
|
||||
+23
@@ -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'
|
||||
Reference in New Issue
Block a user