From e8060b56835b3708705efbe296668a439d117154 Mon Sep 17 00:00:00 2001 From: Harold Kim Date: Sat, 15 Aug 2026 13:35:51 -0600 Subject: [PATCH 1/4] Setup markdown file, create waybionic_control package structure --- docs/control/can_control_architecture.md | 51 +++++++++++++++++++ waybionic_control/package.xml | 22 ++++++++ waybionic_control/resource/waybionic_control | 0 waybionic_control/setup.cfg | 4 ++ waybionic_control/setup.py | 29 +++++++++++ waybionic_control/test/test_copyright.py | 25 +++++++++ waybionic_control/test/test_flake8.py | 25 +++++++++ waybionic_control/test/test_pep257.py | 23 +++++++++ .../waybionic_control/__init__.py | 0 .../waybionic_control/node/__init__.py | 0 .../waybionic_control/protocol/__init__.py | 0 .../waybionic_control/transport/__init__.py | 0 12 files changed, 179 insertions(+) create mode 100644 docs/control/can_control_architecture.md create mode 100644 waybionic_control/package.xml create mode 100644 waybionic_control/resource/waybionic_control create mode 100644 waybionic_control/setup.cfg create mode 100644 waybionic_control/setup.py create mode 100644 waybionic_control/test/test_copyright.py create mode 100644 waybionic_control/test/test_flake8.py create mode 100644 waybionic_control/test/test_pep257.py create mode 100644 waybionic_control/waybionic_control/__init__.py create mode 100644 waybionic_control/waybionic_control/node/__init__.py create mode 100644 waybionic_control/waybionic_control/protocol/__init__.py create mode 100644 waybionic_control/waybionic_control/transport/__init__.py diff --git a/docs/control/can_control_architecture.md b/docs/control/can_control_architecture.md new file mode 100644 index 0000000..c5f1723 --- /dev/null +++ b/docs/control/can_control_architecture.md @@ -0,0 +1,51 @@ +# CAN Control Architecture + +## End-to-End Network Diagram +This defines the physical and logical boundaries of the Waybionic 6-DOF arm control network. + +```text +[ Doctor Controller / Network ] + | (Ethernet/Wi-Fi) + v +[ Robot Computer (Host) ] ---(USB3/GigE)---> [ Cameras ] + |-- ROS 2 High-Level + |-- ros2_control Hardware Interface + |-- SocketCAN Abstraction + | (CAN-FD) + v +[ Logical CAN Channel (vcan0 / can0) ] + |--> [ Joint 1 Node ] + |--> [ Joint 2 Node ] + |--> [ Joint 3 Node ] + |--> [ Joint 4 Node ] + |--> [ Joint 5 Node ] + |--> [ Joint 6 Node ] + +* Note: Power, E-Stop, Motor Enable, and Hardware Safety loops operate on a completely separate hardware layer from the CAN bus. +``` + +## Responsibilities +* **Host (Robot PC):** Computes kinematics, trajectories, and safety limits. Sends high-level position/velocity targets to the bus. Decodes joint feedback and publishes `sensor_msgs/msg/JointState`. Monitors CAN heartbeat/health and publishes to `/diagnostics`. **Does not generate individual step pulses.** +* **Joint Nodes (Drives):** Close the local motor control loops (PID). Convert target pos/vel into actual motor currents/steps. Broadcast current position, velocity, and health/heartbeat back to the CAN bus. + +## Protocol Evaluation: `ros2_canopen` vs. Direct SocketCAN +**1. `ros2_canopen` (CiA 402)** +* **Pros:** Highly standardized. Plug-and-play if we purchase off-the-shelf (COTS) smart actuators that natively run the CANopen CiA 402 motion profile. +* **Cons:** Massive overhead. The CANopen state machine is complex, and the SDO/PDO mapping can be rigid and difficult to debug. + +**2. Direct SocketCAN (Custom Protocol)** +* **Pros:** Extremely low overhead. Allows us to fully utilize CAN-FD's 64-byte payload to pack pos/vel/health into single frames. +* **Cons:** Requires us to define our own frame IDs and data packing. + +**Recommendation & Decision:** +We will proceed with **Direct SocketCAN** wrapped in a clean, hardware-independent abstraction layer. +* *If Electrical designs custom joint-controller PCBs:* We have the lightweight protocol we need. +* *If Mechanical chooses COTS CANopen motors:* Our abstraction layer allows us to seamlessly swap the transport backend to `ros2_canopen` later without rewriting the core `ros2_control` logic. +*(Provisional 6-node IDs and data layouts will be used until hardware is finalized).* + +## Useful Websites +- https://www.csselectronics.com/pages/can-fd-flexible-data-rate-intro +- https://docs.kernel.org/networking/can.html +- https://github.com/linux-can/socketcand +- https://github.com/ros-industrial/ros2_canopen +- https://docs.openarm.dev/api-reference/can/ \ No newline at end of file diff --git a/waybionic_control/package.xml b/waybionic_control/package.xml new file mode 100644 index 0000000..f0eeced --- /dev/null +++ b/waybionic_control/package.xml @@ -0,0 +1,22 @@ + + + + waybionic_control + 0.0.0 + TODO: Package description + hoodu + TODO: License declaration + + rclpy + sensor_msgs + diagnostic_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/waybionic_control/resource/waybionic_control b/waybionic_control/resource/waybionic_control new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_control/setup.cfg b/waybionic_control/setup.cfg new file mode 100644 index 0000000..2678702 --- /dev/null +++ b/waybionic_control/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/waybionic_control +[install] +install_scripts=$base/lib/waybionic_control diff --git a/waybionic_control/setup.py b/waybionic_control/setup.py new file mode 100644 index 0000000..60302ab --- /dev/null +++ b/waybionic_control/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup + +package_name = 'waybionic_control' + +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='hoodu', + maintainer_email='harold.kim@ucalgary.ca', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/waybionic_control/test/test_copyright.py b/waybionic_control/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/waybionic_control/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/waybionic_control/test/test_flake8.py b/waybionic_control/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/waybionic_control/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/waybionic_control/test/test_pep257.py b/waybionic_control/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/waybionic_control/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/waybionic_control/waybionic_control/__init__.py b/waybionic_control/waybionic_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_control/waybionic_control/node/__init__.py b/waybionic_control/waybionic_control/node/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_control/waybionic_control/protocol/__init__.py b/waybionic_control/waybionic_control/protocol/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_control/waybionic_control/transport/__init__.py b/waybionic_control/waybionic_control/transport/__init__.py new file mode 100644 index 0000000..e69de29 From 6697586459a034d35c6e975fda2accdf3dbac0be Mon Sep 17 00:00:00 2001 From: Harold Kim Date: Sat, 15 Aug 2026 13:56:50 -0600 Subject: [PATCH 2/4] partial work of task 3&4 --- scripts/setup_vcan.sh | 16 ++++++++++++++++ waybionic_control/package.xml | 2 ++ 2 files changed, 18 insertions(+) create mode 100755 scripts/setup_vcan.sh diff --git a/scripts/setup_vcan.sh b/scripts/setup_vcan.sh new file mode 100755 index 0000000..056c5d7 --- /dev/null +++ b/scripts/setup_vcan.sh @@ -0,0 +1,16 @@ +#!/bin/bash +set -e + +echo "=== Setting up Virtual CAN interface (vcan0) ===" + +# Load the virtual CAN kernel module +sudo modprobe vcan + +# Create the vcan0 link (ignore error if it already exists) +sudo ip link add dev vcan0 type vcan 2>/dev/null || true + +# Bring the interface up +sudo ip link set up vcan0 + +echo "✅ vcan0 is up and running!" +echo "You can monitor traffic by running: candump vcan0" diff --git a/waybionic_control/package.xml b/waybionic_control/package.xml index f0eeced..4ccf4b7 100644 --- a/waybionic_control/package.xml +++ b/waybionic_control/package.xml @@ -11,6 +11,8 @@ sensor_msgs diagnostic_msgs + python3-can + ament_copyright ament_flake8 ament_pep257 From 8cef6f0f7e164416de57a2961e7ddaca02249382 Mon Sep 17 00:00:00 2001 From: Harold Kim Date: Sat, 29 Aug 2026 13:37:02 -0600 Subject: [PATCH 3/4] Fix tests so they all pass --- waybionic_control/setup.py | 2 + waybionic_control/test/test_can_control.py | 46 +++++++++++ .../waybionic_control/node/can_host.py | 79 +++++++++++++++++++ .../waybionic_control/node/mock_drives.py | 50 ++++++++++++ 4 files changed, 177 insertions(+) create mode 100644 waybionic_control/test/test_can_control.py create mode 100644 waybionic_control/waybionic_control/node/can_host.py create mode 100644 waybionic_control/waybionic_control/node/mock_drives.py diff --git a/waybionic_control/setup.py b/waybionic_control/setup.py index 60302ab..0ab07ca 100644 --- a/waybionic_control/setup.py +++ b/waybionic_control/setup.py @@ -24,6 +24,8 @@ }, entry_points={ 'console_scripts': [ + 'mock_drives = waybionic_control.node.mock_drives:main', + 'can_host = waybionic_control.node.can_host:main' ], }, ) diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py new file mode 100644 index 0000000..5e9d954 --- /dev/null +++ b/waybionic_control/test/test_can_control.py @@ -0,0 +1,46 @@ +import time +import unittest + +import can +import rclpy + +from waybionic_control.node.can_host import CanHostNode + + +class TestCanControlLogic(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = CanHostNode() + + def tearDown(self): + self.node.destroy_node() + + def test_six_node_configuration_and_stale_detection(self): + self.assertEqual(len(self.node.last_seen), 6) + + self.node.last_seen[1] = time.time() + self.node.last_seen[2] = time.time() - 10.0 + + self.node.publish_diagnostics() + + self.assertTrue(time.time() - self.node.last_seen[2] > 0.5) + self.assertTrue(time.time() - self.node.last_seen[1] < 0.5) + + def test_invalid_mappings(self): + try: + msg = can.Message(arbitration_id=0x999, data=b'\x00\x00', is_extended_id=False) + ignored = not (0x101 <= msg.arbitration_id <= 0x106) + self.assertTrue(ignored) + except Exception as e: + self.fail(f'Node crashed on invalid mapping: {e}') + + +if __name__ == '__main__': + unittest.main() diff --git a/waybionic_control/waybionic_control/node/can_host.py b/waybionic_control/waybionic_control/node/can_host.py new file mode 100644 index 0000000..95c2b03 --- /dev/null +++ b/waybionic_control/waybionic_control/node/can_host.py @@ -0,0 +1,79 @@ +import struct +import time + +import can +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +class CanHostNode(Node): + def __init__(self): + super().__init__('can_host') + + self.joint_pub = self.create_publisher(JointState, '/joint_states', 10) + self.diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 10) + + self.get_logger().info('Host connecting to software virtual CAN bus...') + self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1') + + self.last_seen = {i: 0.0 for i in range(1, 7)} + + self.create_timer(0.05, self.read_bus) # 20 Hz read loop + self.create_timer(1.0, self.publish_diagnostics) # 1 Hz diag loop + self.get_logger().info('Host node started. Listening for joint data.') + + def read_bus(self): + while True: + msg = self.bus.recv(0.0) + if msg is None: + break + + if 0x101 <= msg.arbitration_id <= 0x106: + joint_id = msg.arbitration_id - 0x100 + self.last_seen[joint_id] = time.time() + + if len(msg.data) == 5: + position, health = struct.unpack(' 0.5: + status.level = DiagnosticStatus.ERROR + status.message = 'STALE (No heartbeat)' + else: + status.level = DiagnosticStatus.OK + status.message = 'OK' + + diag_array.status.append(status) + + self.diag_pub.publish(diag_array) + + +def main(args=None): + rclpy.init(args=args) + node = CanHostNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/waybionic_control/waybionic_control/node/mock_drives.py b/waybionic_control/waybionic_control/node/mock_drives.py new file mode 100644 index 0000000..dc8e728 --- /dev/null +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -0,0 +1,50 @@ +import struct + +import can +import rclpy +from rclpy.node import Node + + +class MockDrivesNode(Node): + def __init__(self): + super().__init__('mock_drives') + self.declare_parameter('simulate_stale_joint', False) + + self.get_logger().info('Connecting to software virtual CAN bus...') + self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1') + + self.timer = self.create_timer(0.1, self.timer_callback) # 10 Hz + self.count = 0 + self.get_logger().info('Mock drives started. Broadcasting 6 joints at 10Hz.') + + def timer_callback(self): + simulate_stale = self.get_parameter('simulate_stale_joint').value + + for joint_id in range(1, 7): + if simulate_stale and joint_id == 6 and self.count > 30: + continue + + fake_position = 0.0 + health_status = 1 + data = struct.pack(' Date: Sat, 29 Aug 2026 14:15:44 -0600 Subject: [PATCH 4/4] Fix automated flows and tests to pass cleanly --- waybionic_control/test/test_can_control.py | 19 +++- .../waybionic_control/node/can_host.py | 87 ++++++++++++++++--- .../waybionic_control/node/mock_drives.py | 54 ++++++++++-- .../waybionic_control/protocol/codec.py | 28 ++++++ 4 files changed, 167 insertions(+), 21 deletions(-) create mode 100644 waybionic_control/waybionic_control/protocol/codec.py diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py index 5e9d954..ec36c87 100644 --- a/waybionic_control/test/test_can_control.py +++ b/waybionic_control/test/test_can_control.py @@ -5,6 +5,7 @@ import rclpy from waybionic_control.node.can_host import CanHostNode +from waybionic_control.protocol import codec class TestCanControlLogic(unittest.TestCase): @@ -22,6 +23,20 @@ def setUp(self): def tearDown(self): self.node.destroy_node() + def test_codec_packing(self): + # Verify 8-byte command packing + cmd_data = codec.encode_target_command(1.5, -0.5) + self.assertEqual(len(cmd_data), 8) + pos, vel = codec.decode_target_command(cmd_data) + self.assertAlmostEqual(pos, 1.5, places=4) + self.assertAlmostEqual(vel, -0.5, places=4) + + # Verify 10-byte state packing (CAN-FD) + state_data = codec.encode_joint_state(3.14, 0.0, 1, 0xAA) + self.assertEqual(len(state_data), 10) + p, v, h, f = codec.decode_joint_state(state_data) + self.assertEqual(f, 0xAA) + def test_six_node_configuration_and_stale_detection(self): self.assertEqual(len(self.node.last_seen), 6) @@ -36,7 +51,9 @@ def test_six_node_configuration_and_stale_detection(self): def test_invalid_mappings(self): try: msg = can.Message(arbitration_id=0x999, data=b'\x00\x00', is_extended_id=False) - ignored = not (0x101 <= msg.arbitration_id <= 0x106) + ignored = not ( + codec.STATE_BASE_ID + 1 <= msg.arbitration_id <= codec.STATE_BASE_ID + 6 + ) self.assertTrue(ignored) except Exception as e: self.fail(f'Node crashed on invalid mapping: {e}') diff --git a/waybionic_control/waybionic_control/node/can_host.py b/waybionic_control/waybionic_control/node/can_host.py index 95c2b03..4323b39 100644 --- a/waybionic_control/waybionic_control/node/can_host.py +++ b/waybionic_control/waybionic_control/node/can_host.py @@ -1,12 +1,13 @@ -import struct import time import can -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState +from waybionic_control.protocol import codec + class CanHostNode(Node): def __init__(self): @@ -15,14 +16,43 @@ def __init__(self): self.joint_pub = self.create_publisher(JointState, '/joint_states', 10) self.diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 10) + # Subscribe to incoming commands from the high-level ROS 2 system + self.cmd_sub = self.create_subscription( + JointState, '/joint_commands', self.command_callback, 10) + self.get_logger().info('Host connecting to software virtual CAN bus...') self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1') + # State tracking for diagnostics self.last_seen = {i: 0.0 for i in range(1, 7)} + self.faults = {i: 0 for i in range(1, 7)} + self.last_cmd_time = 0.0 self.create_timer(0.05, self.read_bus) # 20 Hz read loop self.create_timer(1.0, self.publish_diagnostics) # 1 Hz diag loop - self.get_logger().info('Host node started. Listening for joint data.') + self.get_logger().info('Host node started. Ready for bidirectional CAN.') + + def command_callback(self, msg): + self.last_cmd_time = time.time() + # Parse the incoming ROS command and send it down the CAN bus + for i, name in enumerate(msg.name): + if name.startswith('joint_'): + try: + joint_id = int(name.split('_')[1]) + if 1 <= joint_id <= 6: + target_pos = msg.position[i] if i < len(msg.position) else 0.0 + target_vel = msg.velocity[i] if i < len(msg.velocity) else 0.0 + + data = codec.encode_target_command(target_pos, target_vel) + can_msg = can.Message( + arbitration_id=codec.CMD_BASE_ID + joint_id, + data=data, + is_extended_id=False, + is_fd=True + ) + self.bus.send(can_msg) + except (ValueError, IndexError, can.CanError) as e: + self.get_logger().error(f'Command error: {e}') def read_bus(self): while True: @@ -30,34 +60,67 @@ def read_bus(self): if msg is None: break - if 0x101 <= msg.arbitration_id <= 0x106: - joint_id = msg.arbitration_id - 0x100 + if codec.STATE_BASE_ID + 1 <= msg.arbitration_id <= codec.STATE_BASE_ID + 6: + joint_id = msg.arbitration_id - codec.STATE_BASE_ID self.last_seen[joint_id] = time.time() - if len(msg.data) == 5: - position, health = struct.unpack(' 1.0: + cmd_stat.level = DiagnosticStatus.WARN + cmd_stat.message = f'STALE COMMANDS ({cmd_age:.1f}s ago)' + else: + cmd_stat.level = DiagnosticStatus.OK + cmd_stat.message = f'ACTIVE ({cmd_age:.1f}s ago)' + diag_array.status.append(cmd_stat) + + # 3. Individual Joint Status for joint_id in range(1, 7): status = DiagnosticStatus() - status.name = f'can.bus: Joint {joint_id} Heartbeat' + status.name = f'can.bus: Joint {joint_id} Health' status.hardware_id = f'joint_{joint_id}' + # Add raw fault code as key/value pair + status.values.append( + KeyValue(key='fault_code', value=hex(self.faults[joint_id])) + ) + if current_time - self.last_seen[joint_id] > 0.5: status.level = DiagnosticStatus.ERROR status.message = 'STALE (No heartbeat)' + elif self.faults[joint_id] != 0: + status.level = DiagnosticStatus.ERROR + status.message = f'HARDWARE FAULT (Code: {hex(self.faults[joint_id])})' else: status.level = DiagnosticStatus.OK status.message = 'OK' diff --git a/waybionic_control/waybionic_control/node/mock_drives.py b/waybionic_control/waybionic_control/node/mock_drives.py index dc8e728..5d12446 100644 --- a/waybionic_control/waybionic_control/node/mock_drives.py +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -1,34 +1,72 @@ -import struct - import can import rclpy from rclpy.node import Node +from waybionic_control.protocol import codec + class MockDrivesNode(Node): def __init__(self): super().__init__('mock_drives') - self.declare_parameter('simulate_stale_joint', False) + self.declare_parameter('simulate_faults', True) self.get_logger().info('Connecting to software virtual CAN bus...') self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1') + self.positions = {i: 0.0 for i in range(1, 7)} + self.velocities = {i: 0.0 for i in range(1, 7)} + self.targets = {i: 0.0 for i in range(1, 7)} + self.timer = self.create_timer(0.1, self.timer_callback) # 10 Hz self.count = 0 self.get_logger().info('Mock drives started. Broadcasting 6 joints at 10Hz.') def timer_callback(self): - simulate_stale = self.get_parameter('simulate_stale_joint').value + simulate_faults = self.get_parameter('simulate_faults').value + + # 1. Read incoming command frames from the host + while True: + msg = self.bus.recv(0.0) + if msg is None: + break + if codec.CMD_BASE_ID + 1 <= msg.arbitration_id <= codec.CMD_BASE_ID + 6: + joint_id = msg.arbitration_id - codec.CMD_BASE_ID + target_pos, target_vel = codec.decode_target_command(msg.data) + self.targets[joint_id] = target_pos + # 2. Simulate movement and broadcast state frames back to the host for joint_id in range(1, 7): - if simulate_stale and joint_id == 6 and self.count > 30: + # Simulate STALE fault (Joint 6 dies after 30 ticks) + if simulate_faults and joint_id == 6 and self.count > 30: continue - fake_position = 0.0 + # Basic simulation: smoothly move toward the target position + diff = self.targets[joint_id] - self.positions[joint_id] + self.velocities[joint_id] = diff * 2.0 + self.positions[joint_id] += diff * 0.5 + health_status = 1 - data = struct.pack(' 50: + health_status = 0 + fault_code = 0xAA # Fake error code (e.g., Motor Overcurrent) + + # Pack 10 bytes of data (CAN-FD allows > 8 bytes) + data = codec.encode_joint_state( + self.positions[joint_id], + self.velocities[joint_id], + health_status, + fault_code + ) - msg = can.Message(arbitration_id=0x100 + joint_id, data=data, is_extended_id=False) + msg = can.Message( + arbitration_id=codec.STATE_BASE_ID + joint_id, + data=data, + is_extended_id=False, + is_fd=True + ) try: self.bus.send(msg) diff --git a/waybionic_control/waybionic_control/protocol/codec.py b/waybionic_control/waybionic_control/protocol/codec.py new file mode 100644 index 0000000..a8708bd --- /dev/null +++ b/waybionic_control/waybionic_control/protocol/codec.py @@ -0,0 +1,28 @@ +import struct + + +# Provisional CAN IDs +STATE_BASE_ID = 0x100 +CMD_BASE_ID = 0x200 + + +def encode_target_command(position, velocity): + # Pack 2 floats (8 bytes total) + return struct.pack('= 8: + return struct.unpack('= 10: + return struct.unpack('