From e027c58f676ee8c443addb37a7a0626a233a6a80 Mon Sep 17 00:00:00 2001
From: dev01 <dev01@example.com>
Date: Sun, 7 Jun 2026 13:33:14 +0800
Subject: [PATCH] feat: add chassis_control_pkg with serial communication node

ChassisNode communicates via /dev/ttyCH341USB0 at 115200 bps:
- Send 0xA7 command frames (auto/manual/debug mode switching)
- Send 0xA6 manual control frames (motor_speed_base + angle_velocity)
- Send 0xA8 debug tuning frames (speed/yaw/position PID params)
- Receive 0xA9 feedforward frames from chassis MCU
- Subscribe: /cmd_chassis, /chassis/mode, /web/chassis_pid_config
- Publish: /chassis/feedforward (angle_velocity for gimbal feedforward)
---
 .../chassis_control_pkg/__init__.py           |   0
 .../chassis_control_pkg/chassis_node.py       | 206 ++++++++++++++++++
 workspace/src/chassis_control_pkg/package.xml |  21 ++
 .../resource/chassis_control_pkg              |   0
 workspace/src/chassis_control_pkg/setup.cfg   |   4 +
 workspace/src/chassis_control_pkg/setup.py    |  28 +++
 .../test/test_copyright.py                    |  24 ++
 .../chassis_control_pkg/test/test_flake8.py   |  25 +++
 .../chassis_control_pkg/test/test_pep257.py   |  23 ++
 9 files changed, 331 insertions(+)
 create mode 100644 workspace/src/chassis_control_pkg/chassis_control_pkg/__init__.py
 create mode 100644 workspace/src/chassis_control_pkg/chassis_control_pkg/chassis_node.py
 create mode 100644 workspace/src/chassis_control_pkg/package.xml
 create mode 100644 workspace/src/chassis_control_pkg/resource/chassis_control_pkg
 create mode 100644 workspace/src/chassis_control_pkg/setup.cfg
 create mode 100644 workspace/src/chassis_control_pkg/setup.py
 create mode 100644 workspace/src/chassis_control_pkg/test/test_copyright.py
 create mode 100644 workspace/src/chassis_control_pkg/test/test_flake8.py
 create mode 100644 workspace/src/chassis_control_pkg/test/test_pep257.py

diff --git a/workspace/src/chassis_control_pkg/chassis_control_pkg/__init__.py b/workspace/src/chassis_control_pkg/chassis_control_pkg/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/workspace/src/chassis_control_pkg/chassis_control_pkg/chassis_node.py b/workspace/src/chassis_control_pkg/chassis_control_pkg/chassis_node.py
new file mode 100644
index 0000000..81879e3
--- /dev/null
+++ b/workspace/src/chassis_control_pkg/chassis_control_pkg/chassis_node.py
@@ -0,0 +1,206 @@
+#!/usr/bin/env python3
+import json
+import struct
+import threading
+
+import serial
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32, Float32MultiArray, String
+
+CMD_AUTO = 0x01
+CMD_MANUAL = 0x02
+CMD_DEBUG = 0x03
+
+FRAME_SOF_COMMAND = 0xA7
+FRAME_SOF_MANUAL = 0xA6
+FRAME_SOF_DEBUG = 0xA8
+FRAME_SOF_FEEDFORWARD = 0xA9
+
+SERIAL_READ_TIMEOUT = 0.1
+
+
+def _checksum(data: bytearray) -> int:
+    return sum(data) & 0xFF
+
+
+def _checksum_match(data: bytearray) -> bool:
+    return len(data) > 1 and data[-1] == _checksum(data[:-1])
+
+
+class ChassisNode(Node):
+    def __init__(self):
+        super().__init__('chassis_node')
+
+        self.declare_parameter('serial_port', '/dev/ttyCH341USB0')
+        self.declare_parameter('baud_rate', 115200)
+        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=SERIAL_READ_TIMEOUT,
+        )
+        self.get_logger().info(f'Serial port {port} opened at {baud} baud')
+
+        self._cmd_chassis_sub = self.create_subscription(
+            Float32MultiArray, '/cmd_chassis', self._cmd_chassis_callback, 10
+        )
+        self._mode_sub = self.create_subscription(
+            String, '/chassis/mode', self._mode_callback, 10
+        )
+        self._pid_config_sub = self.create_subscription(
+            String, '/web/chassis_pid_config', self._pid_config_callback, 10
+        )
+
+        self._feedforward_pub = self.create_publisher(
+            Float32, '/chassis/feedforward', 10
+        )
+
+        self._current_mode = 'auto'
+
+        self._read_thread = threading.Thread(
+            target=self._serial_read_loop, daemon=True
+        )
+        self._read_thread.start()
+
+        self.get_logger().info(
+            'ChassisNode ready - subs: /cmd_chassis, /chassis/mode, '
+            '/web/chassis_pid_config, pub: /chassis/feedforward'
+        )
+
+    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_chassis_callback(self, msg: Float32MultiArray):
+        if self._current_mode != 'manual':
+            return
+        if len(msg.data) < 2:
+            self.get_logger().error(
+                f'/cmd_chassis requires 2 values, got {len(msg.data)}')
+            return
+
+        motor_speed_base = int(max(-50, min(50, msg.data[0])))
+        angle_velocity = float(msg.data[1])
+
+        data = bytearray()
+        data.append(FRAME_SOF_MANUAL)
+        data.extend(struct.pack('<h', motor_speed_base))
+        data.extend(struct.pack('<f', angle_velocity))
+        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
+        else:
+            self.get_logger().warn(f'Unknown chassis mode: {mode}')
+            return
+
+        self._current_mode = mode
+        self.get_logger().info(
+            f'Sending chassis command: 0xA7 cmd=0x{cmd:02X} ({mode})')
+        data = bytearray([FRAME_SOF_COMMAND, cmd])
+        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 chassis PID JSON: {e}')
+            return
+
+        speed = pid.get('speed', {})
+        angle = pid.get('angle', {})
+        position = pid.get('position', {})
+
+        self.get_logger().info('Sending chassis PID debug tuning frame')
+        data = bytearray()
+        data.append(FRAME_SOF_DEBUG)
+        data.extend(struct.pack(
+            '<fff',
+            speed.get('kp', 0.0), speed.get('ki', 0.0),
+            speed.get('kd', 0.0),
+        ))
+        data.extend(struct.pack(
+            '<fff',
+            angle.get('kp', 0.0), angle.get('ki', 0.0),
+            angle.get('kd', 0.0),
+        ))
+        data.extend(struct.pack(
+            '<fff',
+            position.get('kp', 0.0), position.get('ki', 0.0),
+            position.get('kd', 0.0),
+        ))
+        data.append(_checksum(data))
+        self._write_frame(data)
+
+    def _serial_read_loop(self):
+        buf = bytearray()
+        while self._ser and self._ser.is_open:
+            try:
+                chunk = self._ser.read(64)
+            except serial.SerialException:
+                continue
+            if not chunk:
+                continue
+            buf.extend(chunk)
+            buf = self._parse_frames(buf)
+
+    def _parse_frames(self, buf: bytearray) -> bytearray:
+        while len(buf) > 0:
+            sof = buf[0]
+            if sof == FRAME_SOF_FEEDFORWARD:
+                frame_len = 6
+                if len(buf) < frame_len:
+                    break
+                frame = buf[:frame_len]
+                if _checksum_match(frame):
+                    angle_vel = struct.unpack('<f', frame[1:5])[0]
+                    msg = Float32()
+                    msg.data = angle_vel
+                    self._feedforward_pub.publish(msg)
+                else:
+                    self.get_logger().debug(
+                        f'Checksum mismatch on 0xA9 frame: '
+                        f'{frame.hex(" ")}'
+                    )
+                buf = buf[frame_len:]
+            else:
+                buf = buf[1:]
+        return buf
+
+    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 = ChassisNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        try:
+            rclpy.shutdown()
+        except RuntimeError:
+            pass
diff --git a/workspace/src/chassis_control_pkg/package.xml b/workspace/src/chassis_control_pkg/package.xml
new file mode 100644
index 0000000..e5a40c3
--- /dev/null
+++ b/workspace/src/chassis_control_pkg/package.xml
@@ -0,0 +1,21 @@
+<?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>chassis_control_pkg</name>
+  <version>0.0.0</version>
+  <description>Chassis serial communication node (UART 115200 8N1, /dev/ttyCH341USB0)</description>
+  <maintainer email="dev01@todo.todo">dev01</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</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>
diff --git a/workspace/src/chassis_control_pkg/resource/chassis_control_pkg b/workspace/src/chassis_control_pkg/resource/chassis_control_pkg
new file mode 100644
index 0000000..e69de29
diff --git a/workspace/src/chassis_control_pkg/setup.cfg b/workspace/src/chassis_control_pkg/setup.cfg
new file mode 100644
index 0000000..27823c8
--- /dev/null
+++ b/workspace/src/chassis_control_pkg/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/chassis_control_pkg
+[install]
+install_scripts=$base/lib/chassis_control_pkg
diff --git a/workspace/src/chassis_control_pkg/setup.py b/workspace/src/chassis_control_pkg/setup.py
new file mode 100644
index 0000000..ff0c1cb
--- /dev/null
+++ b/workspace/src/chassis_control_pkg/setup.py
@@ -0,0 +1,28 @@
+from setuptools import find_packages, setup
+
+package_name = 'chassis_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='Chassis serial communication node',
+    license='TODO: License declaration',
+    extras_require={
+        'test': ['pytest'],
+    },
+    entry_points={
+        'console_scripts': [
+            'chassis_node = chassis_control_pkg.chassis_node:main'
+        ],
+    },
+)
diff --git a/workspace/src/chassis_control_pkg/test/test_copyright.py b/workspace/src/chassis_control_pkg/test/test_copyright.py
new file mode 100644
index 0000000..b992092
--- /dev/null
+++ b/workspace/src/chassis_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/chassis_control_pkg/test/test_flake8.py b/workspace/src/chassis_control_pkg/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/workspace/src/chassis_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/chassis_control_pkg/test/test_pep257.py b/workspace/src/chassis_control_pkg/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/workspace/src/chassis_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'