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/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 new file mode 100644 index 0000000..4ccf4b7 --- /dev/null +++ b/waybionic_control/package.xml @@ -0,0 +1,24 @@ + + + + waybionic_control + 0.0.0 + TODO: Package description + hoodu + TODO: License declaration + + rclpy + sensor_msgs + diagnostic_msgs + + python3-can + + 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..0ab07ca --- /dev/null +++ b/waybionic_control/setup.py @@ -0,0 +1,31 @@ +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': [ + '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..ec36c87 --- /dev/null +++ b/waybionic_control/test/test_can_control.py @@ -0,0 +1,63 @@ +import time +import unittest + +import can +import rclpy + +from waybionic_control.node.can_host import CanHostNode +from waybionic_control.protocol import codec + + +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_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) + + 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 ( + 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}') + + +if __name__ == '__main__': + unittest.main() 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/node/can_host.py b/waybionic_control/waybionic_control/node/can_host.py new file mode 100644 index 0000000..4323b39 --- /dev/null +++ b/waybionic_control/waybionic_control/node/can_host.py @@ -0,0 +1,142 @@ +import time + +import can +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): + super().__init__('can_host') + + 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. 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: + 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) + + def publish_joint_state(self, joint_id, pos, vel): + js = JointState() + js.header.stamp = self.get_clock().now().to_msg() + js.name = [f'joint_{joint_id}'] + js.position = [pos] + js.velocity = [vel] + self.joint_pub.publish(js) + + def publish_diagnostics(self): + diag_array = DiagnosticArray() + 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' + ) + + 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: + cmd_stat.level = DiagnosticStatus.WARN + cmd_stat.message = 'NO COMMANDS RECEIVED YET' + elif cmd_age > 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} 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' + + 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..5d12446 --- /dev/null +++ b/waybionic_control/waybionic_control/node/mock_drives.py @@ -0,0 +1,88 @@ +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_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_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): + # 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 + + 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) + + # 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=codec.STATE_BASE_ID + joint_id, + data=data, + is_extended_id=False, + is_fd=True + ) + + try: + self.bus.send(msg) + except can.CanError as e: + self.get_logger().error(f'CAN error: {e}') + + self.count += 1 + + +def main(args=None): + rclpy.init(args=args) + node = MockDrivesNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() 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/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('