Fix critical bugs and improve hardware interface robustness

.github/workflows/ci.yml

@@ -0,0 +1,56 @@
+name: CI
+
+on:
+  push:
+    branches: ["main", "master"]
+  pull_request:
+    branches: ["main", "master"]
+
+jobs:
+  lint:
+    name: Python Lint (flake8)
+    runs-on: ubuntu-latest
+    permissions:
+      contents: read
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Set up Python
+        uses: actions/setup-python@v5
+        with:
+          python-version: "3.10"
+
+      - name: Install lint dependencies
+        run: pip install flake8
+
+      - name: Run flake8 on pilla_hw package
+        run: |
+          cd ros_code/ros_ws/src/pilla_hw
+          python3 -m flake8 pilla_hw/ --count --statistics \
+            --select=F --extend-ignore=F401
+
+      - name: Run flake8 on ODrive CAN scripts
+        run: |
+          python3 -m flake8 odrive_scripts/can_code/ --count --statistics \
+            --select=F --extend-ignore=F401
+
+  can-imports:
+    name: Validate CAN Library Imports
+    runs-on: ubuntu-latest
+    permissions:
+      contents: read
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Set up Python
+        uses: actions/setup-python@v5
+        with:
+          python-version: "3.10"
+
+      - name: Install CAN dependencies
+        run: pip install cantools python-can pyserial
+
+      - name: Validate CAN library imports
+        run: |
+          cd odrive_scripts/can_code
+          python3 -c "from can_joint import CanJoint; import cantools; print('CAN libraries import OK')"

arduino_sketches/imu_over_serial/imu_over_serial.ino

@@ -6,7 +6,7 @@
 //create JSON document
 StaticJsonDocument<200> doc; // allocates 200 bytes for JSON (could reduce?)
 
-const unsigned long interval = 10; // 20ms for 50Hz
+const unsigned long interval = 10; // 10ms for 100Hz
 unsigned long lastUpdate = 0;
 bool useRefreshRate = true; // use refresh rate if available
 

odrive_scripts/can_code/can_joint.py

@@ -71,13 +71,12 @@ def set_limits(self):
         self.can_bus.send(msg)
 
     def test_movement(self):
-        target = 0
         t0 = time.monotonic()
         for i in range(1,50):
             setpoint = 4.0 * math.sin((time.monotonic() - t0)*2)
             print("goto " + str(setpoint))
             data = self.can_db.encode_message('Axis0_Set_Input_Pos', {'Input_Pos':setpoint, 'Vel_FF':0.0, 'Torque_FF':0.0})
-            msg = can.Message(arbitration_id=axisID << 5 | 0x00C, data=data, is_extended_id=False)
+            msg = can.Message(arbitration_id=self.axisID << 5 | 0x00C, data=data, is_extended_id=False)
             self.can_bus.send(msg)
             time.sleep(0.01)
 

ros_code/ros_ws/src/pilla_hw/launch/champ_pilla_launch.py

@@ -1,11 +1,13 @@
 import os
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     this_package_name = 'pilla_hw'
     this_package = FindPackageShare(this_package_name)
@@ -16,40 +18,52 @@ def generate_launch_description():
         [this_package, 'launch', 'champ_arduino_imu_launch.py']
     )
     champ_bringup_launch_path = PathJoinSubstitution(
-    [FindPackageShare('champ_config'), 'launch', 'bringup.launch.py']
+        [FindPackageShare('champ_config'), 'launch', 'bringup.launch.py']
     )
     champ_teleop_launch_path = PathJoinSubstitution(
-    [FindPackageShare('pilla_teleop'), 'launch', 'teleop.launch.py']
+        [FindPackageShare('pilla_teleop'), 'launch', 'teleop.launch.py']
     )
     return LaunchDescription([
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(odrive_launch_path),
         ),
 
         DeclareLaunchArgument(
-            name='rviz', 
+            name='rviz',
             default_value='true',
             description='Run rviz'
         ),
 
         DeclareLaunchArgument(
-            name='robot_name', 
+            name='robot_name',
             default_value='champ',
             description='Set robot name for multi robot'
         ),
 
         DeclareLaunchArgument(
-            name='sim', 
+            name='sim',
             default_value='false',
             description='Enable use_sim_time to true'
         ),
 
         DeclareLaunchArgument(
-            name='hardware_connected', 
+            name='hardware_connected',
             default_value='false',
             description='Set to true if connected to a physical robot'
         ),
 
+        DeclareLaunchArgument(
+            name='use_joy',
+            default_value='true',
+            description='Use joystick (true) or keyboard (false) for teleoperation'
+        ),
+
+        DeclareLaunchArgument(
+            name='use_imu',
+            default_value='true',
+            description='Launch Arduino IMU node'
+        ),
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(champ_bringup_launch_path),
             launch_arguments={
@@ -64,20 +78,20 @@ def generate_launch_description():
             PythonLaunchDescriptionSource(champ_teleop_launch_path),
             launch_arguments={
                 'use_sim_time': LaunchConfiguration('sim'),
-                'use_joy': 'true',
+                'use_joy': LaunchConfiguration('use_joy'),
                 'dev': '/dev/input/js0'
             }.items()
         ),
-        
+
         Node(
             package="pilla_hw",
             executable="odrive_interface",
             name="pilla_odrive_interface",
             namespace="pilla",
         ),
 
-        # IncludeLaunchDescription(
-        #     PythonLaunchDescriptionSource(pilla_arduino_imu_launch_path),
-        # ),
-
-])
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(pilla_arduino_imu_launch_path),
+            condition=IfCondition(LaunchConfiguration('use_imu')),
+        ),
+    ])

ros_code/ros_ws/src/pilla_hw/launch/odrive_nodes_launch.py

@@ -1,134 +1,21 @@
-import os
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.substitutions import FindPackageShare
 from launch_ros.actions import Node
 
-def generate_launch_description():
-    return LaunchDescription([
 
-        # Add odrive_can_node
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node0",
-            namespace="odrive_axis0",
-            parameters=[
-            {"node_id": 0},
-            {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node1",
-            namespace="odrive_axis1",
-            parameters=[
-            {"node_id": 1},
-            {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node2",
-            namespace="odrive_axis2",
-            parameters=[
-            {"node_id": 2},
-            {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node3",
-            namespace="odrive_axis3",
-            parameters=[
-            {"node_id": 3},
-            {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node4",
-            namespace="odrive_axis4",
-            parameters=[
-            {"node_id": 4},
-            {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node5",
-            namespace="odrive_axis5",
-            parameters=[
-                {"node_id": 5},
-                {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node6",
-            namespace="odrive_axis6",
-            parameters=[
-                {"node_id": 6},
-                {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node7",
-            namespace="odrive_axis7",
-            parameters=[
-                {"node_id": 7},
-                {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node8",
-            namespace="odrive_axis8",
-            parameters=[
-                {"node_id": 8},
-                {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node9",
-            namespace="odrive_axis9",
-            parameters=[
-                {"node_id": 9},
-                {"interface": "can0"}
-            ]
-        ),
-        Node(
-            package="odrive_can",
-            executable="odrive_can_node",
-            name="odrive_can_node10",
-            namespace="odrive_axis10",
-            parameters=[
-                {"node_id": 10},
-                {"interface": "can0"}
-            ]
-        ),
+def generate_launch_description():
+    num_axes = 12
+    nodes = [
         Node(
             package="odrive_can",
             executable="odrive_can_node",
-            name="odrive_can_node11",
-            namespace="odrive_axis11",
+            name=f"odrive_can_node{i}",
+            namespace=f"odrive_axis{i}",
             parameters=[
-                {"node_id": 11},
-                {"interface": "can0"}
+                {"node_id": i},
+                {"interface": "can0"},
             ]
-        ),
-
-])
+        )
+        for i in range(num_axes)
+    ]
+    return LaunchDescription(nodes)
+

ros_code/ros_ws/src/pilla_hw/package.xml

@@ -7,6 +7,14 @@
   <maintainer email="vish@wychar.com">vish</maintainer>
   <license>Apache-2.0</license>
 
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <exec_depend>python3-serial</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>