From e8060b56835b3708705efbe296668a439d117154 Mon Sep 17 00:00:00 2001 From: Harold Kim Date: Sat, 15 Aug 2026 13:35:51 -0600 Subject: [PATCH 01/10] 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 02/10] 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 03/10] 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 04/10] 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(' Date: Sat, 5 Sep 2026 14:53:21 -0600 Subject: [PATCH 05/10] Address comments from coderabbitai --- waybionic_control/package.xml | 4 +- waybionic_control/test/test_can_control.py | 79 ++++++++++++++++--- waybionic_control/test/test_copyright.py | 2 - .../waybionic_control/node/can_host.py | 60 ++++++++------ .../waybionic_control/node/mock_drives.py | 40 +++++++--- .../waybionic_control/protocol/codec.py | 19 ++++- 6 files changed, 149 insertions(+), 55 deletions(-) diff --git a/waybionic_control/package.xml b/waybionic_control/package.xml index 4ccf4b7..f2dacc6 100644 --- a/waybionic_control/package.xml +++ b/waybionic_control/package.xml @@ -4,8 +4,8 @@ waybionic_control 0.0.0 TODO: Package description - hoodu - TODO: License declaration + Harold Kim + Apache-2.0 rclpy sensor_msgs diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py index ec36c87..5e53095 100644 --- a/waybionic_control/test/test_can_control.py +++ b/waybionic_control/test/test_can_control.py @@ -1,14 +1,32 @@ +# Copyright 2026 Waybionic +# +# 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 time import unittest +from unittest.mock import MagicMock, patch import can +from diagnostic_msgs.msg import DiagnosticStatus import rclpy +from sensor_msgs.msg import JointState from waybionic_control.node.can_host import CanHostNode from waybionic_control.protocol import codec class TestCanControlLogic(unittest.TestCase): + @classmethod def setUpClass(cls): rclpy.init() @@ -18,45 +36,80 @@ def tearDownClass(cls): rclpy.shutdown() def setUp(self): + # Prevent the node from opening real network sockets during tests + self.bus_patcher = patch('can.interface.Bus') + self.mock_bus = self.bus_patcher.start() self.node = CanHostNode() def tearDown(self): self.node.destroy_node() + self.bus_patcher.stop() 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_incomplete_command_rejected(self): + js = JointState() + js.name = ['joint_1'] + js.position = [] + + self.node.command_callback(js) + self.node.bus.send.assert_not_called() + + msg_with_pos = JointState() + msg_with_pos.name = ['joint_1'] + msg_with_pos.position = [1.5] + self.node.command_callback(msg_with_pos) + self.node.bus.send.assert_called_once() + + def test_invalid_mappings(self): + msg = can.Message( + arbitration_id=0x999, data=b'\x00\x00', is_extended_id=False) + self.node.bus.recv.side_effect = [msg, None] + + last_seen = dict(self.node.last_seen) + + try: + self.node.read_bus() + except Exception as e: + self.fail(f'Node crashed on invalid mapping: {e}') + self.assertEqual(self.node.last_seen, last_seen) + + def test_short_payloads(self): + with self.assertRaises(ValueError): + codec.decode_target_command(b'\x00' * 7) + + with self.assertRaises(ValueError): + codec.decode_joint_state(b'\x00' * 9) + 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.diag_pub.publish = MagicMock() + 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) + self.node.diag_pub.publish.assert_called_once() + published_msg = self.node.diag_pub.publish.call_args[0][0] + statuses = {s.hardware_id: s for s in published_msg.status} - def test_invalid_mappings(self): - try: - msg = can.Message(arbitration_id=0x999, data=b'\x00\x00', is_extended_id=False) - 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}') + self.assertEqual(statuses['joint_1'].level, DiagnosticStatus.OK) + self.assertEqual(statuses['joint_1'].message, 'OK') + + self.assertEqual(statuses['joint_2'].level, DiagnosticStatus.ERROR) + self.assertEqual(statuses['joint_2'].message, 'STALE (No heartbeat)') if __name__ == '__main__': diff --git a/waybionic_control/test/test_copyright.py b/waybionic_control/test/test_copyright.py index 97a3919..cc8ff03 100644 --- a/waybionic_control/test/test_copyright.py +++ b/waybionic_control/test/test_copyright.py @@ -16,8 +16,6 @@ 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(): diff --git a/waybionic_control/waybionic_control/node/can_host.py b/waybionic_control/waybionic_control/node/can_host.py index 4323b39..72ba613 100644 --- a/waybionic_control/waybionic_control/node/can_host.py +++ b/waybionic_control/waybionic_control/node/can_host.py @@ -1,3 +1,17 @@ +# Copyright 2026 Waybionic +# +# 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 time import can @@ -16,31 +30,35 @@ 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') + self.declare_parameter('can_interface', 'vcan0') + can_interface = self.get_parameter('can_interface').value + + self.get_logger().info(f'Host connecting to CAN bus on {can_interface}...') + self.bus = can.interface.Bus(bustype='socketcan', channel=can_interface) - # 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.create_timer(0.05, self.read_bus) + self.create_timer(1.0, self.publish_diagnostics) 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_'): + if i >= len(msg.position): + self.get_logger().warning(f'Rejecting command {name}: missing position') + continue + 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_pos = msg.position[i] target_vel = msg.velocity[i] if i < len(msg.velocity) else 0.0 data = codec.encode_target_command(target_pos, target_vel) @@ -55,18 +73,21 @@ def command_callback(self, msg): self.get_logger().error(f'Command error: {e}') def read_bus(self): - while True: + # Process max 100 messages per tick to prevent infinite blocking + for _ in range(100): msg = self.bus.recv(0.0) if msg is None: break 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() - - pos, vel, health, fault = codec.decode_joint_state(msg.data) - self.faults[joint_id] = fault - self.publish_joint_state(joint_id, pos, vel) + try: + pos, vel, health, fault = codec.decode_joint_state(msg.data) + self.last_seen[joint_id] = time.time() + self.faults[joint_id] = fault + self.publish_joint_state(joint_id, pos, vel) + except ValueError as e: + self.get_logger().warning(f'Ignored bad state: {e}') def publish_joint_state(self, joint_id, pos, vel): js = JointState() @@ -81,16 +102,13 @@ def publish_diagnostics(self): diag_array.header.stamp = self.get_clock().now().to_msg() current_time = time.time() - # 1. Bus Alive Status bus_stat = DiagnosticStatus( - name='can.bus: Link Status', - level=DiagnosticStatus.OK, - message='ACTIVE' + name='can.bus: Link Status', + level=DiagnosticStatus.OK, + message='ACTIVE' ) - diag_array.status.append(bus_stat) - # 2. Command Age Status cmd_stat = DiagnosticStatus(name='can.bus: Command Age') cmd_age = current_time - self.last_cmd_time if self.last_cmd_time == 0.0: @@ -104,13 +122,11 @@ def publish_diagnostics(self): 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} 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])) ) diff --git a/waybionic_control/waybionic_control/node/mock_drives.py b/waybionic_control/waybionic_control/node/mock_drives.py index 5d12446..75e7bbb 100644 --- a/waybionic_control/waybionic_control/node/mock_drives.py +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -1,3 +1,17 @@ +# Copyright 2026 Waybionic +# +# 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 can import rclpy from rclpy.node import Node @@ -9,38 +23,40 @@ class MockDrivesNode(Node): def __init__(self): super().__init__('mock_drives') self.declare_parameter('simulate_faults', True) + self.declare_parameter('can_interface', 'vcan0') - self.get_logger().info('Connecting to software virtual CAN bus...') - self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1') + can_interface = self.get_parameter('can_interface').value + self.get_logger().info(f'Connecting to virtual CAN bus on {can_interface}...') + self.bus = can.interface.Bus(bustype='socketcan', channel=can_interface) 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.timer = self.create_timer(0.1, self.timer_callback) self.count = 0 self.get_logger().info('Mock drives started. Broadcasting 6 joints at 10Hz.') def timer_callback(self): simulate_faults = self.get_parameter('simulate_faults').value - # 1. Read incoming command frames from the host - while True: + # Process max 100 messages per tick to prevent infinite blocking + for _ in range(100): 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 + try: + target_pos, target_vel = codec.decode_target_command(msg.data) + self.targets[joint_id] = target_pos + except ValueError as e: + self.get_logger().warning(f'Ignored bad command: {e}') - # 2. Simulate movement and broadcast state frames back to the host for joint_id in range(1, 7): - # Simulate STALE fault (Joint 6 dies after 30 ticks) if simulate_faults and joint_id == 6 and self.count > 30: continue - # 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 @@ -48,12 +64,10 @@ def timer_callback(self): health_status = 1 fault_code = 0 - # Simulate HARDWARE FAULT (Joint 4 throws error 0xAA after 50 ticks) if simulate_faults and joint_id == 4 and self.count > 50: health_status = 0 - fault_code = 0xAA # Fake error code (e.g., Motor Overcurrent) + fault_code = 0xAA - # Pack 10 bytes of data (CAN-FD allows > 8 bytes) data = codec.encode_joint_state( self.positions[joint_id], self.velocities[joint_id], diff --git a/waybionic_control/waybionic_control/protocol/codec.py b/waybionic_control/waybionic_control/protocol/codec.py index a8708bd..b8aba5c 100644 --- a/waybionic_control/waybionic_control/protocol/codec.py +++ b/waybionic_control/waybionic_control/protocol/codec.py @@ -1,5 +1,18 @@ -import struct +# Copyright 2026 Waybionic +# +# 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 struct # Provisional CAN IDs STATE_BASE_ID = 0x100 @@ -14,7 +27,7 @@ def encode_target_command(position, velocity): def decode_target_command(data): if len(data) >= 8: return struct.unpack('= 10: return struct.unpack(' Date: Sat, 5 Sep 2026 15:10:49 -0600 Subject: [PATCH 06/10] Make fixes --- .../waybionic_control/node/can_host.py | 2 ++ .../waybionic_control/node/mock_drives.py | 3 ++ .../waybionic_control/protocol/codec.py | 34 +++++++++++++++++-- 3 files changed, 37 insertions(+), 2 deletions(-) diff --git a/waybionic_control/waybionic_control/node/can_host.py b/waybionic_control/waybionic_control/node/can_host.py index 72ba613..89038f6 100644 --- a/waybionic_control/waybionic_control/node/can_host.py +++ b/waybionic_control/waybionic_control/node/can_host.py @@ -24,6 +24,8 @@ class CanHostNode(Node): + """Node that translates ROS JointState messages to CAN frames and monitors health.""" + def __init__(self): super().__init__('can_host') diff --git a/waybionic_control/waybionic_control/node/mock_drives.py b/waybionic_control/waybionic_control/node/mock_drives.py index 75e7bbb..bd8ba9d 100644 --- a/waybionic_control/waybionic_control/node/mock_drives.py +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -20,7 +20,10 @@ class MockDrivesNode(Node): + """Node simulating 6 CAN-based joint controllers.""" + def __init__(self): + """Initialize the MockDrivesNode and connect to the virtual CAN bus.""" super().__init__('mock_drives') self.declare_parameter('simulate_faults', True) self.declare_parameter('can_interface', 'vcan0') diff --git a/waybionic_control/waybionic_control/protocol/codec.py b/waybionic_control/waybionic_control/protocol/codec.py index b8aba5c..af67be4 100644 --- a/waybionic_control/waybionic_control/protocol/codec.py +++ b/waybionic_control/waybionic_control/protocol/codec.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Protocol codec for encoding and decoding provisional Waybionic CAN-FD frames.""" + import struct # Provisional CAN IDs @@ -20,22 +22,50 @@ def encode_target_command(position, velocity): - # Pack 2 floats (8 bytes total) + """ + Encode a target position and velocity into a CAN command frame. + + :param position: Target position. + :param velocity: Target velocity. + :return: 8-byte packed payload. + """ return struct.pack('= 8: return struct.unpack('= 10: return struct.unpack(' Date: Sat, 12 Sep 2026 13:08:13 -0600 Subject: [PATCH 07/10] Address PR comments, fix test files --- waybionic_control/test/test_can_control.py | 42 ++++++++++++++++++- .../waybionic_control/node/can_host.py | 15 ++++++- .../waybionic_control/node/mock_drives.py | 7 +++- .../waybionic_control/protocol/codec.py | 15 ++++++- 4 files changed, 74 insertions(+), 5 deletions(-) diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py index 5e53095..75d907d 100644 --- a/waybionic_control/test/test_can_control.py +++ b/waybionic_control/test/test_can_control.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import struct import time import unittest from unittest.mock import MagicMock, patch @@ -36,7 +37,6 @@ def tearDownClass(cls): rclpy.shutdown() def setUp(self): - # Prevent the node from opening real network sockets during tests self.bus_patcher = patch('can.interface.Bus') self.mock_bus = self.bus_patcher.start() self.node = CanHostNode() @@ -91,6 +91,20 @@ def test_short_payloads(self): with self.assertRaises(ValueError): codec.decode_joint_state(b'\x00' * 9) + def test_non_finite_values_rejected(self): + with self.assertRaises(ValueError): + codec.encode_target_command(float('nan'), 0.0) + with self.assertRaises(ValueError): + codec.encode_joint_state(0.0, float('inf'), 1, 0) + + bad_cmd = struct.pack(' 0.5: status.level = DiagnosticStatus.ERROR @@ -139,6 +149,9 @@ def publish_diagnostics(self): elif self.faults[joint_id] != 0: status.level = DiagnosticStatus.ERROR status.message = f'HARDWARE FAULT (Code: {hex(self.faults[joint_id])})' + elif self.healths[joint_id] == 0: + status.level = DiagnosticStatus.ERROR + status.message = 'UNHEALTHY (health=0, no fault code)' 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 bd8ba9d..d42b5fa 100644 --- a/waybionic_control/waybionic_control/node/mock_drives.py +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -30,7 +30,12 @@ def __init__(self): can_interface = self.get_parameter('can_interface').value self.get_logger().info(f'Connecting to virtual CAN bus on {can_interface}...') - self.bus = can.interface.Bus(bustype='socketcan', channel=can_interface) + + try: + self.bus = can.interface.Bus(bustype='socketcan', channel=can_interface, fd=True) + except Exception as e: + self.get_logger().warning(f'SocketCAN failed ({e}), falling back to udp_multicast') + self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1', fd=True) self.positions = {i: 0.0 for i in range(1, 7)} self.velocities = {i: 0.0 for i in range(1, 7)} diff --git a/waybionic_control/waybionic_control/protocol/codec.py b/waybionic_control/waybionic_control/protocol/codec.py index af67be4..867c75e 100644 --- a/waybionic_control/waybionic_control/protocol/codec.py +++ b/waybionic_control/waybionic_control/protocol/codec.py @@ -14,6 +14,7 @@ """Protocol codec for encoding and decoding provisional Waybionic CAN-FD frames.""" +import math import struct # Provisional CAN IDs @@ -29,6 +30,8 @@ def encode_target_command(position, velocity): :param velocity: Target velocity. :return: 8-byte packed payload. """ + if not (math.isfinite(position) and math.isfinite(velocity)): + raise ValueError('Command contains NaN or Inf values') return struct.pack('= 8: - return struct.unpack('= 10: - return struct.unpack(' Date: Sat, 12 Sep 2026 15:04:39 -0600 Subject: [PATCH 08/10] Fix package.xml files, add health checks --- waybionic_bringup/launch/can_demo.launch.py | 61 +++++++++++++++++++ .../launch/ground_station.launch.py | 4 -- waybionic_bringup/package.xml | 1 + waybionic_control/package.xml | 9 ++- waybionic_control/setup.py | 4 +- waybionic_control/test/test_can_control.py | 49 +++++++++++++++ .../waybionic_control/node/mock_drives.py | 14 +++-- 7 files changed, 129 insertions(+), 13 deletions(-) create mode 100644 waybionic_bringup/launch/can_demo.launch.py diff --git a/waybionic_bringup/launch/can_demo.launch.py b/waybionic_bringup/launch/can_demo.launch.py new file mode 100644 index 0000000..258030d --- /dev/null +++ b/waybionic_bringup/launch/can_demo.launch.py @@ -0,0 +1,61 @@ +# Copyright 2026 Waybionic +# +# 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 launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + can_interface_arg = DeclareLaunchArgument( + 'can_interface', default_value='vcan0', + description='SocketCAN interface; falls back to udp_multicast if unavailable') + + simulate_faults_arg = DeclareLaunchArgument( + 'simulate_faults', default_value='true', + description='Inject the provisional joint 4 / joint 6 fault scenarios') + + start_mock_drives_arg = DeclareLaunchArgument( + 'start_mock_drives', default_value='true', + description='Set false when driving real hardware on the bus') + + can_host_node = Node( + package='waybionic_control', + executable='can_host', + name='can_host', + output='screen', + parameters=[{'can_interface': LaunchConfiguration('can_interface')}] + ) + + mock_drives_node = Node( + package='waybionic_control', + executable='mock_drives', + name='mock_drives', + output='screen', + condition=IfCondition(LaunchConfiguration('start_mock_drives')), + parameters=[ + {'can_interface': LaunchConfiguration('can_interface')}, + {'simulate_faults': LaunchConfiguration('simulate_faults')} + ] + ) + + return LaunchDescription([ + can_interface_arg, + simulate_faults_arg, + start_mock_drives_arg, + can_host_node, + mock_drives_node, + ]) diff --git a/waybionic_bringup/launch/ground_station.launch.py b/waybionic_bringup/launch/ground_station.launch.py index 2183b8b..b9b3fab 100644 --- a/waybionic_bringup/launch/ground_station.launch.py +++ b/waybionic_bringup/launch/ground_station.launch.py @@ -28,7 +28,6 @@ def generate_launch_description(): default_rviz_config_path = os.path.join( waybionic_bringup_dir, 'rviz', 'waybionic_unified.rviz') - # --- Declare Launch Arguments --- model_arg = DeclareLaunchArgument( 'model', default_value=default_model_path, description='Absolute path to robot urdf') @@ -63,7 +62,6 @@ def generate_launch_description(): file_check = OpaqueFunction(function=check_files_exist) - # --- Nodes --- robot_description_content = { 'robot_description': Command(['xacro ', LaunchConfiguration('model')]) } @@ -81,7 +79,6 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('use_joint_state_publisher_gui')) ) - # Pass the correct arguments to the temporary publisher temp_diag_pub_node = Node( package='waybionic_rviz_plugins', executable='temporary_diagnostics_publisher.py', name='temp_diag_pub', @@ -93,7 +90,6 @@ def generate_launch_description(): ] ) - # Pass the mock toggles into the RViz node parameters rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', output='screen', arguments=['-d', LaunchConfiguration('rvizconfig')], diff --git a/waybionic_bringup/package.xml b/waybionic_bringup/package.xml index f262ccb..d35d763 100644 --- a/waybionic_bringup/package.xml +++ b/waybionic_bringup/package.xml @@ -13,6 +13,7 @@ xacro waybionic_description waybionic_rviz_plugins + waybionic_control launch launch_ros robot_state_publisher diff --git a/waybionic_control/package.xml b/waybionic_control/package.xml index f2dacc6..c5914b3 100644 --- a/waybionic_control/package.xml +++ b/waybionic_control/package.xml @@ -3,15 +3,18 @@ waybionic_control 0.0.0 - TODO: Package description + + CAN-FD host and mock drive nodes for WayBionic ground station. + Bridges ROS 2 JointState commands to a provisional CAN-FD protocol over vcan0 with udp_multicast fallback + Publishes joint feedback and per-joint health diagnostics. + Harold Kim Apache-2.0 rclpy sensor_msgs diagnostic_msgs - - python3-can + python3-can ament_copyright ament_flake8 diff --git a/waybionic_control/setup.py b/waybionic_control/setup.py index 0ab07ca..c15da1c 100644 --- a/waybionic_control/setup.py +++ b/waybionic_control/setup.py @@ -15,8 +15,8 @@ zip_safe=True, maintainer='hoodu', maintainer_email='harold.kim@ucalgary.ca', - description='TODO: Package description', - license='TODO: License declaration', + description='CAN-FD host and mock drive nodes for WayBionic ground station', + license='Apache-2.0', extras_require={ 'test': [ 'pytest', diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py index 75d907d..9a53125 100644 --- a/waybionic_control/test/test_can_control.py +++ b/waybionic_control/test/test_can_control.py @@ -151,6 +151,55 @@ def test_health_byte_preserved_from_bus(self): self.assertEqual(self.node.healths[3], 0) + def test_non_finite_feedback_leaves_state_unchanged(self): + self.node.last_seen[2] = 0.0 + self.node.faults[2] = 0x11 + self.node.healths[2] = 1 + + bad_state = struct.pack(' 30: + if simulate_faults and joint_id == 6 and 30 < self.count <= 70: continue diff = self.targets[joint_id] - self.positions[joint_id] @@ -72,9 +72,15 @@ def timer_callback(self): health_status = 1 fault_code = 0 - if simulate_faults and joint_id == 4 and self.count > 50: - health_status = 0 - fault_code = 0xAA + if simulate_faults: + # arbitrary values for now + # scale values up if longer capture desired + if joint_id == 4 and 50 < self.count <= 90: + health_status = 0 + fault_code = 0xAA + elif joint_id == 5 and 60 < self.count <= 100: + health_status = 0 + fault_code = 0 data = codec.encode_joint_state( self.positions[joint_id], From 841f2227b7cfc327c6c178747a2f23e6dc6044e0 Mon Sep 17 00:00:00 2001 From: Harold Kim Date: Sat, 19 Sep 2026 13:54:41 -0600 Subject: [PATCH 09/10] Addressing comments: Reject values outside wire format's range in codec.py, Make alternate transport choice explicit and report failure in can_host.py --- waybionic_bringup/launch/can_demo.launch.py | 14 +++- waybionic_control/test/test_can_control.py | 69 ++++++++++++++++ .../waybionic_control/node/can_host.py | 80 ++++++++++++++----- .../waybionic_control/node/mock_drives.py | 3 + .../waybionic_control/protocol/codec.py | 39 +++++++-- 5 files changed, 176 insertions(+), 29 deletions(-) diff --git a/waybionic_bringup/launch/can_demo.launch.py b/waybionic_bringup/launch/can_demo.launch.py index 258030d..3973491 100644 --- a/waybionic_bringup/launch/can_demo.launch.py +++ b/waybionic_bringup/launch/can_demo.launch.py @@ -32,12 +32,20 @@ def generate_launch_description(): 'start_mock_drives', default_value='true', description='Set false when driving real hardware on the bus') + transport_arg = DeclareLaunchArgument( + 'transport', default_value='socketcan', + description='Transport: socketcan (default) or udp_multicast.' + ) + can_host_node = Node( package='waybionic_control', executable='can_host', name='can_host', output='screen', - parameters=[{'can_interface': LaunchConfiguration('can_interface')}] + parameters=[ + {'can_interface': LaunchConfiguration('can_interface')}, + {'transport': LaunchConfiguration('transport')}, + ] ) mock_drives_node = Node( @@ -48,7 +56,8 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('start_mock_drives')), parameters=[ {'can_interface': LaunchConfiguration('can_interface')}, - {'simulate_faults': LaunchConfiguration('simulate_faults')} + {'simulate_faults': LaunchConfiguration('simulate_faults')}, + {'transport': LaunchConfiguration('transport')}, ] ) @@ -56,6 +65,7 @@ def generate_launch_description(): can_interface_arg, simulate_faults_arg, start_mock_drives_arg, + transport_arg, can_host_node, mock_drives_node, ]) diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py index 9a53125..23483e0 100644 --- a/waybionic_control/test/test_can_control.py +++ b/waybionic_control/test/test_can_control.py @@ -20,6 +20,7 @@ import can from diagnostic_msgs.msg import DiagnosticStatus import rclpy +from rclpy.parameter import Parameter from sensor_msgs.msg import JointState from waybionic_control.node.can_host import CanHostNode @@ -200,6 +201,74 @@ def test_non_finite_command_rejected_then_valid_accepted(self): self.node.command_callback(good) self.node.bus.send.assert_called_once() + def test_float32_overflow_rejected_then_valid_accepted(self): + with self.assertRaises(ValueError): + codec.encode_target_command(1e40, 0.0) + with self.assertRaises(ValueError): + codec.encode_joint_state(0.0, -1e40, 1, 0) + + huge = JointState() + huge.name = ['joint_1'] + huge.position = [1e40] + + try: + self.node.command_callback(huge) + except OverflowError: + self.fail('OverflowError escaped command_callback') + self.node.bus.send.assert_not_called() + + good_cmd = JointState() + good_cmd.name = ['joint_1'] + good_cmd.position = [1.5] + self.node.command_callback(good_cmd) + self.node.bus.send.assert_called_once() + + def test_float32_boundary_accepted(self): + data = codec.encode_target_command(codec.FLOAT32_MAX, -codec.FLOAT32_MAX) + self.assertEqual(len(data), 8) + + def test_socketcan_failure_degrades_node(self): + self.bus_patcher.stop() + with patch('can.interface.Bus', side_effect=OSError('No such device')): + node = CanHostNode() + self.mock_bus = self.bus_patcher.start() + + try: + self.assertIsNone(node.bus) + node.diag_pub.publish = MagicMock() + node.publish_diagnostics() + + published = node.diag_pub.publish.call_args[0][0] + link = [s for s in published.status + if s.name == 'can.bus: Link Status'][0] + self.assertEqual(link.level, DiagnosticStatus.ERROR) + self.assertIn('DOWN', link.message) + + js = JointState() + js.name = ['joint_1'] + js.position = [1.5] + node.command_callback(js) + node.read_bus() + finally: + node.destroy_node() + + def test_explicit_udp_transport_selected(self): + node = CanHostNode(parameter_overrides=[ + Parameter('transport', Parameter.Type.STRING, 'udp_multicast')]) + try: + self.assertEqual( + self.mock_bus.call_args.kwargs['bustype'], 'udp_multicast') + node.diag_pub.publish = MagicMock() + node.publish_diagnostics() + + published = node.diag_pub.publish.call_args[0][0] + link = [s for s in published.status + if s.name == 'can.bus: Link Status'][0] + self.assertEqual(link.level, DiagnosticStatus.OK) + self.assertIn('udp_multicast', link.message) + finally: + node.destroy_node() + 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 index 01537a2..41083d5 100644 --- a/waybionic_control/waybionic_control/node/can_host.py +++ b/waybionic_control/waybionic_control/node/can_host.py @@ -26,8 +26,8 @@ class CanHostNode(Node): """Node that translates ROS JointState messages to CAN frames and monitors health.""" - def __init__(self): - super().__init__('can_host') + def __init__(self, **kwargs): + super().__init__('can_host', **kwargs) self.joint_pub = self.create_publisher(JointState, '/joint_states', 10) self.diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 10) @@ -36,15 +36,39 @@ def __init__(self): JointState, '/joint_commands', self.command_callback, 10) self.declare_parameter('can_interface', 'vcan0') - can_interface = self.get_parameter('can_interface').value - - self.get_logger().info(f'Host connecting to CAN bus on {can_interface}...') + self.declare_parameter('transport', 'socketcan') - try: - self.bus = can.interface.Bus(bustype='socketcan', channel=can_interface, fd=True) - except Exception as e: - self.get_logger().warning(f'SocketCAN failed ({e}), falling back to udp_multicast') - self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1', fd=True) + can_interface = self.get_parameter('can_interface').value + self.transport = self.get_parameter('transport').value + self.bus = None + self.bus_error = None + + if self.transport == 'socketcan': + try: + self.bus = can.interface.Bus( + bustype='socketcan', channel=can_interface, fd=True) + self.get_logger().info(f'SocketCAN active on {can_interface}') + except (can.CanError, OSError) as e: + self.bus_error = str(e) + self.get_logger().error( + f'SocketCAN init failed on {can_interface}: {e}. Node is ' + 'degraded; no frames sent or received. Set ' + 'transport:=udp_multicast to use UDP explicitly.') + elif self.transport == 'udp_multicast': + try: + self.bus = can.interface.Bus( + bustype='udp_multicast', channel='224.0.0.1', fd=True) + self.get_logger().warning( + 'udp_multicast transport selected. This is NOT a physical ' + 'CAN link and must not be used for hardware validation.') + except (can.CanError, OSError) as e: + self.bus_error = str(e) + self.get_logger().error(f'udp_multicast init failed: {e}') + else: + self.bus_error = f'unknown transport {self.transport!r}' + self.get_logger().error( + f'Unknown transport {self.transport!r}; ' + "expected 'socketcan' or 'udp_multicast'.") self.last_seen = {i: 0.0 for i in range(1, 7)} self.faults = {i: 0 for i in range(1, 7)} @@ -56,6 +80,12 @@ def __init__(self): self.get_logger().info('Host node started. Ready for bidirectional CAN.') def command_callback(self, msg): + if self.bus is None: + self.get_logger().warning( + 'Command dropped: no CAN transport available', + throttle_duration_sec=5.0) + return + self.last_cmd_time = time.time() for i, name in enumerate(msg.name): if name.startswith('joint_'): @@ -81,6 +111,9 @@ def command_callback(self, msg): self.get_logger().error(f'Command error: {e}') def read_bus(self): + if self.bus is None: + return + # Process max 100 messages per tick to prevent infinite blocking for _ in range(100): msg = self.bus.recv(0.0) @@ -111,11 +144,20 @@ def publish_diagnostics(self): diag_array.header.stamp = self.get_clock().now().to_msg() current_time = time.time() - bus_stat = DiagnosticStatus( - name='can.bus: Link Status', - level=DiagnosticStatus.OK, - message='ACTIVE' - ) + if self.bus is None: + bus_stat = DiagnosticStatus( + name='can.bus: Link Status', + level=DiagnosticStatus.ERROR, + message=f'DOWN ({self.transport} init failed: {self.bus_error})' + ) + else: + bus_stat = DiagnosticStatus( + name='can.bus: Link Status', + level=DiagnosticStatus.OK, + message=f'ACTIVE ({self.transport})' + ) + bus_stat.values.append( + KeyValue(key='transport', value=str(self.transport))) diag_array.status.append(bus_stat) cmd_stat = DiagnosticStatus(name='can.bus: Command Age') @@ -136,12 +178,8 @@ def publish_diagnostics(self): status.name = f'can.bus: Joint {joint_id} Health' status.hardware_id = f'joint_{joint_id}' - status.values.append( - KeyValue(key='fault_code', value=hex(self.faults[joint_id])) - ) - status.values.append( - KeyValue(key='health', value=str(self.healths[joint_id])) - ) + status.values.append(KeyValue(key='fault_code', value=hex(self.faults[joint_id]))) + status.values.append(KeyValue(key='health', value=str(self.healths[joint_id]))) if current_time - self.last_seen[joint_id] > 0.5: status.level = DiagnosticStatus.ERROR diff --git a/waybionic_control/waybionic_control/node/mock_drives.py b/waybionic_control/waybionic_control/node/mock_drives.py index d8bb239..cb6d1f4 100644 --- a/waybionic_control/waybionic_control/node/mock_drives.py +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -46,6 +46,9 @@ def __init__(self): self.get_logger().info('Mock drives started. Broadcasting 6 joints at 10Hz.') def timer_callback(self): + if self.bus is None: + return + simulate_faults = self.get_parameter('simulate_faults').value # Process max 100 messages per tick to prevent infinite blocking diff --git a/waybionic_control/waybionic_control/protocol/codec.py b/waybionic_control/waybionic_control/protocol/codec.py index 867c75e..01010d9 100644 --- a/waybionic_control/waybionic_control/protocol/codec.py +++ b/waybionic_control/waybionic_control/protocol/codec.py @@ -21,6 +21,24 @@ STATE_BASE_ID = 0x100 CMD_BASE_ID = 0x200 +# Maximum magnitude representable in IEEE-754 binary32 (' FLOAT32_MAX: + raise ValueError( + f'{label} ({value!r}) exceeds float32 wire range +/-{FLOAT32_MAX:g}') + def encode_target_command(position, velocity): """ @@ -29,10 +47,14 @@ def encode_target_command(position, velocity): :param position: Target position. :param velocity: Target velocity. :return: 8-byte packed payload. + :raises ValueError: If a value is NaN, Inf, or outside float32 range. """ - if not (math.isfinite(position) and math.isfinite(velocity)): - raise ValueError('Command contains NaN or Inf values') - return struct.pack(' Date: Sat, 19 Sep 2026 14:26:39 -0600 Subject: [PATCH 10/10] Address comments --- waybionic_control/test/test_can_control.py | 51 ++++++++++++++ .../waybionic_control/node/mock_drives.py | 68 ++++++++++++------- 2 files changed, 94 insertions(+), 25 deletions(-) diff --git a/waybionic_control/test/test_can_control.py b/waybionic_control/test/test_can_control.py index 23483e0..891da9f 100644 --- a/waybionic_control/test/test_can_control.py +++ b/waybionic_control/test/test_can_control.py @@ -24,6 +24,7 @@ from sensor_msgs.msg import JointState from waybionic_control.node.can_host import CanHostNode +from waybionic_control.node.mock_drives import MockDrivesNode from waybionic_control.protocol import codec @@ -269,6 +270,56 @@ def test_explicit_udp_transport_selected(self): finally: node.destroy_node() + def test_fault_and_stale_recovery(self): + self.node.diag_pub.publish = MagicMock() + + self.node.last_seen[4] = time.time() - 10.0 + self.node.publish_diagnostics() + statuses = {s.hardware_id: s + for s in self.node.diag_pub.publish.call_args[0][0].status} + self.assertEqual(statuses['joint_4'].message, 'STALE (No heartbeat)') + + faulted = codec.encode_joint_state(0.5, 0.0, 1, 0xAA) + self.node.bus.recv.side_effect = [ + can.Message(arbitration_id=codec.STATE_BASE_ID + 4, + data=faulted, is_extended_id=False), None] + self.node.read_bus() + self.node.publish_diagnostics() + statuses = {s.hardware_id: s + for s in self.node.diag_pub.publish.call_args[0][0].status} + self.assertEqual(statuses['joint_4'].level, DiagnosticStatus.ERROR) + self.assertIn('HARDWARE FAULT', statuses['joint_4'].message) + + healthy = codec.encode_joint_state(0.5, 0.0, 1, 0) + self.node.bus.recv.side_effect = [ + can.Message(arbitration_id=codec.STATE_BASE_ID + 4, + data=healthy, is_extended_id=False), None] + self.node.read_bus() + self.node.publish_diagnostics() + statuses = {s.hardware_id: s + for s in self.node.diag_pub.publish.call_args[0][0].status} + self.assertEqual(statuses['joint_4'].level, DiagnosticStatus.OK) + self.assertEqual(statuses['joint_4'].message, 'OK') + + def test_mock_drives_explicit_transport(self): + node = MockDrivesNode(parameter_overrides=[ + Parameter('transport', Parameter.Type.STRING, 'udp_multicast')]) + try: + self.assertEqual( + self.mock_bus.call_args.kwargs['bustype'], 'udp_multicast') + finally: + node.destroy_node() + + self.bus_patcher.stop() + with patch('can.interface.Bus', side_effect=OSError('No such device')): + degraded = MockDrivesNode() + self.mock_bus = self.bus_patcher.start() + try: + self.assertIsNone(degraded.bus) + degraded.timer_callback() + finally: + degraded.destroy_node() + if __name__ == '__main__': unittest.main() diff --git a/waybionic_control/waybionic_control/node/mock_drives.py b/waybionic_control/waybionic_control/node/mock_drives.py index cb6d1f4..10ac092 100644 --- a/waybionic_control/waybionic_control/node/mock_drives.py +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -22,20 +22,39 @@ class MockDrivesNode(Node): """Node simulating 6 CAN-based joint controllers.""" - def __init__(self): - """Initialize the MockDrivesNode and connect to the virtual CAN bus.""" - super().__init__('mock_drives') + def __init__(self, **kwargs): + """Initialize the MockDrivesNode and connect to the configured transport.""" + super().__init__('mock_drives', **kwargs) self.declare_parameter('simulate_faults', True) self.declare_parameter('can_interface', 'vcan0') + self.declare_parameter('transport', 'socketcan') can_interface = self.get_parameter('can_interface').value - self.get_logger().info(f'Connecting to virtual CAN bus on {can_interface}...') + self.transport = self.get_parameter('transport').value + self.bus = None - try: - self.bus = can.interface.Bus(bustype='socketcan', channel=can_interface, fd=True) - except Exception as e: - self.get_logger().warning(f'SocketCAN failed ({e}), falling back to udp_multicast') - self.bus = can.interface.Bus(bustype='udp_multicast', channel='224.0.0.1', fd=True) + if self.transport == 'socketcan': + try: + self.bus = can.interface.Bus( + bustype='socketcan', channel=can_interface, fd=True) + self.get_logger().info(f'SocketCAN active on {can_interface}') + except (can.CanError, OSError) as e: + self.get_logger().error( + f'SocketCAN init failed on {can_interface}: {e}. Node is ' + 'degraded; no frames will be published.') + elif self.transport == 'udp_multicast': + try: + self.bus = can.interface.Bus( + bustype='udp_multicast', channel='224.0.0.1', fd=True) + self.get_logger().warning( + 'udp_multicast transport selected. This is NOT a physical ' + 'CAN link and must not be used for hardware validation.') + except (can.CanError, OSError) as e: + self.get_logger().error(f'udp_multicast init failed: {e}') + else: + self.get_logger().error( + f'Unknown transport {self.transport!r}; ' + "expected 'socketcan' or 'udp_multicast'.") self.positions = {i: 0.0 for i in range(1, 7)} self.velocities = {i: 0.0 for i in range(1, 7)} @@ -75,9 +94,8 @@ def timer_callback(self): health_status = 1 fault_code = 0 + # Provisional fault scenarios, pending Electrical confirmation. if simulate_faults: - # arbitrary values for now - # scale values up if longer capture desired if joint_id == 4 and 50 < self.count <= 90: health_status = 0 fault_code = 0xAA @@ -85,22 +103,22 @@ def timer_callback(self): health_status = 0 fault_code = 0 - data = codec.encode_joint_state( - self.positions[joint_id], - self.velocities[joint_id], - health_status, - fault_code - ) - - msg = can.Message( - arbitration_id=codec.STATE_BASE_ID + joint_id, - data=data, - is_extended_id=False, - is_fd=True - ) - try: + data = codec.encode_joint_state( + self.positions[joint_id], + self.velocities[joint_id], + health_status, + fault_code + ) + msg = can.Message( + arbitration_id=codec.STATE_BASE_ID + joint_id, + data=data, + is_extended_id=False, + is_fd=True + ) self.bus.send(msg) + except ValueError as e: + self.get_logger().warning(f'Skipped bad state for joint {joint_id}: {e}') except can.CanError as e: self.get_logger().error(f'CAN error: {e}')