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)
This commit is contained in:
dev01
@@ -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
|
||||
@@ -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>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/chassis_control_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/chassis_control_pkg
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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