Feature/can control foundation

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/

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"

waybionic_control/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>waybionic_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="harold.kim@ucalgary.ca">hoodu</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+
+  <exec_depend>python3-can</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

waybionic_control/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/waybionic_control
+[install]
+install_scripts=$base/lib/waybionic_control

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'
+        ],
+    },
+)

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()

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'

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)

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'