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
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