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_bringup/launch/can_demo.launch.py

@@ -0,0 +1,71 @@
+# 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')
+
+    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')},
+            {'transport': LaunchConfiguration('transport')},
+        ]
+    )
+
+    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')},
+            {'transport': LaunchConfiguration('transport')},
+        ]
+    )
+
+    return LaunchDescription([
+        can_interface_arg,
+        simulate_faults_arg,
+        start_mock_drives_arg,
+        transport_arg,
+        can_host_node,
+        mock_drives_node,
+    ])

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

waybionic_bringup/package.xml

@@ -13,6 +13,7 @@
   <exec_depend>xacro</exec_depend>
   <exec_depend>waybionic_description</exec_depend>
   <exec_depend>waybionic_rviz_plugins</exec_depend>
+  <exec_depend>waybionic_control</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>

waybionic_control/package.xml

@@ -0,0 +1,27 @@
+<?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>
+    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.
+  </description>
+  <maintainer email="harold.kim@ucalgary.ca">Harold Kim</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>python3-can</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='CAN-FD host and mock drive nodes for WayBionic ground station',
+    license='Apache-2.0',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'mock_drives = waybionic_control.node.mock_drives:main',
+            'can_host = waybionic_control.node.can_host:main'
+        ],
+    },
+)