From 557378b69e03dd07b70a4db1c19cfcf62a1476ab Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sat, 21 Mar 2026 17:59:39 +0000 Subject: [PATCH 1/2] Initial plan From c0baf1ee7e1d858190cd0fcc859a8285f9a7a59e Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sat, 21 Mar 2026 18:09:57 +0000 Subject: [PATCH 2/2] fix: bug fixes and hardware interface improvements across the codebase Co-authored-by: Macbull <11361002+Macbull@users.noreply.github.com> Agent-Logs-Url: https://github.com/code-name-57/pillla_hw_code/sessions/d3c7bc54-def8-4ea9-9ca6-201d930b964f --- .github/workflows/ci.yml | 56 +++++++ .../imu_over_serial/imu_over_serial.ino | 2 +- odrive_scripts/can_code/can_joint.py | 3 +- .../src/pilla_hw/launch/champ_pilla_launch.py | 40 +++-- .../pilla_hw/launch/odrive_nodes_launch.py | 137 ++---------------- ros_code/ros_ws/src/pilla_hw/package.xml | 8 + .../pilla_hw/pilla_hw/arduino_interface.py | 121 +++++++++------- ros_code/ros_ws/src/pilla_teleop/package.xml | 13 +- 8 files changed, 181 insertions(+), 199 deletions(-) create mode 100644 .github/workflows/ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..525f716 --- /dev/null +++ b/.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')" diff --git a/arduino_sketches/imu_over_serial/imu_over_serial.ino b/arduino_sketches/imu_over_serial/imu_over_serial.ino index 82a9255..999d6c3 100644 --- a/arduino_sketches/imu_over_serial/imu_over_serial.ino +++ b/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 diff --git a/odrive_scripts/can_code/can_joint.py b/odrive_scripts/can_code/can_joint.py index 92af590..9926170 100644 --- a/odrive_scripts/can_code/can_joint.py +++ b/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) diff --git a/ros_code/ros_ws/src/pilla_hw/launch/champ_pilla_launch.py b/ros_code/ros_ws/src/pilla_hw/launch/champ_pilla_launch.py index 392532e..43a0aa3 100644 --- a/ros_code/ros_ws/src/pilla_hw/launch/champ_pilla_launch.py +++ b/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,10 +18,10 @@ 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( @@ -27,29 +29,41 @@ def generate_launch_description(): ), 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,11 +78,11 @@ 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", @@ -76,8 +90,8 @@ def generate_launch_description(): namespace="pilla", ), - # IncludeLaunchDescription( - # PythonLaunchDescriptionSource(pilla_arduino_imu_launch_path), - # ), - -]) + IncludeLaunchDescription( + PythonLaunchDescriptionSource(pilla_arduino_imu_launch_path), + condition=IfCondition(LaunchConfiguration('use_imu')), + ), + ]) diff --git a/ros_code/ros_ws/src/pilla_hw/launch/odrive_nodes_launch.py b/ros_code/ros_ws/src/pilla_hw/launch/odrive_nodes_launch.py index e7da2ca..e665b23 100644 --- a/ros_code/ros_ws/src/pilla_hw/launch/odrive_nodes_launch.py +++ b/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) + diff --git a/ros_code/ros_ws/src/pilla_hw/package.xml b/ros_code/ros_ws/src/pilla_hw/package.xml index fa5f68f..23ea0fb 100644 --- a/ros_code/ros_ws/src/pilla_hw/package.xml +++ b/ros_code/ros_ws/src/pilla_hw/package.xml @@ -7,6 +7,14 @@ vish Apache-2.0 + rclpy + sensor_msgs + trajectory_msgs + std_msgs + std_srvs + diagnostic_msgs + python3-serial + ament_copyright ament_flake8 ament_pep257 diff --git a/ros_code/ros_ws/src/pilla_hw/pilla_hw/arduino_interface.py b/ros_code/ros_ws/src/pilla_hw/pilla_hw/arduino_interface.py index abc50db..4d6d0d4 100644 --- a/ros_code/ros_ws/src/pilla_hw/pilla_hw/arduino_interface.py +++ b/ros_code/ros_ws/src/pilla_hw/pilla_hw/arduino_interface.py @@ -4,81 +4,98 @@ import json from sensor_msgs.msg import Imu + class ArduinoInterfaceNode(Node): def __init__(self): super().__init__('arduino_node') - # PUBLISHING (to odrive node) + self.declare_parameter('serial_port', '/dev/ttyACM0') + self.declare_parameter('baud_rate', 9600) + + serial_port = self.get_parameter('serial_port').value + baud_rate = self.get_parameter('baud_rate').value + + # PUBLISHING self.publisher_arduino = self.create_publisher( Imu, - 'imu/data_raw', #topic - 1000 # queue size + 'imu/data_raw', + 10 ) + + self.ser = None try: self.ser = serial.Serial( - port='/dev/ttyACM0', # Replace with your serial port - baudrate=9600, + port=serial_port, + baudrate=baud_rate, timeout=1 ) self.get_logger().info(f"Serial port {self.ser.name} opened successfully.") - except serial.SerialException as e: self.get_logger().error(f"Error opening serial port: {e}") - self.read_serial_data() - + self.create_timer(0.02, self.read_serial_data) + def read_serial_data(self): - while rclpy.ok(): - try: - line = self.ser.readline() - if line: - data = json.loads(line) - self.get_logger().info(f"Published IMU data: {data}") - - imu_msg = Imu() - imu_msg.header.stamp = self.get_clock().now().to_msg() - imu_msg.angular_velocity.x = float(data['Gx']) - imu_msg.angular_velocity.y = float(data['Gy']) - imu_msg.angular_velocity.z = float(data['Gz']) - imu_msg.linear_acceleration.x = float(data['Ax']) - imu_msg.linear_acceleration.y = float(data['Ay']) - imu_msg.linear_acceleration.z = float(data['Az']) - - imu_msg.orientation_covariance[0] = -1 # Mark orientation as unknown - imu_msg.orientation_covariance[4] = 1e6 - imu_msg.orientation_covariance[8] = 1e6 - - imu_msg.header.frame_id = "imu_link" - imu_msg.orientation.x = 0.0 - imu_msg.orientation.y = 0.0 - imu_msg.orientation.z = 0.0 - imu_msg.orientation.w = 1.0 - - imu_msg.angular_velocity_covariance[0] = 1e-6 - imu_msg.angular_velocity_covariance[4] = 1e-6 - imu_msg.angular_velocity_covariance[8] = 1e-6 - - imu_msg.linear_acceleration_covariance[0] = 1e-6 - imu_msg.linear_acceleration_covariance[4] = 1e-6 - imu_msg.linear_acceleration_covariance[8] = 1e-6 - self.publisher_arduino.publish(imu_msg) - - except serial.SerialException as e: - self.get_logger().error(f"Error opening serial port: {e}") - finally: - if 'ser' in locals() and self.ser.is_open: - self.ser.close() - self.get_logger().info("Serial port closed.") + if self.ser is None or not self.ser.is_open: + return + try: + line = self.ser.readline() + if not line: + return + data = json.loads(line) + imu_msg = Imu() + imu_msg.header.stamp = self.get_clock().now().to_msg() + imu_msg.header.frame_id = "imu_link" -def main(args=None): + imu_msg.angular_velocity.x = float(data['Gx']) + imu_msg.angular_velocity.y = float(data['Gy']) + imu_msg.angular_velocity.z = float(data['Gz']) + imu_msg.linear_acceleration.x = float(data['Ax']) + imu_msg.linear_acceleration.y = float(data['Ay']) + imu_msg.linear_acceleration.z = float(data['Az']) + + imu_msg.orientation_covariance[0] = -1 # Mark orientation as unknown + imu_msg.orientation_covariance[4] = 1e6 + imu_msg.orientation_covariance[8] = 1e6 + imu_msg.orientation.x = 0.0 + imu_msg.orientation.y = 0.0 + imu_msg.orientation.z = 0.0 + imu_msg.orientation.w = 1.0 + + imu_msg.angular_velocity_covariance[0] = 1e-6 + imu_msg.angular_velocity_covariance[4] = 1e-6 + imu_msg.angular_velocity_covariance[8] = 1e-6 + + imu_msg.linear_acceleration_covariance[0] = 1e-6 + imu_msg.linear_acceleration_covariance[4] = 1e-6 + imu_msg.linear_acceleration_covariance[8] = 1e-6 + + self.publisher_arduino.publish(imu_msg) + self.get_logger().debug(f"Published IMU data: {data}") + + except json.JSONDecodeError as e: + self.get_logger().warning(f"Failed to parse IMU JSON: {e}") + except serial.SerialException as e: + self.get_logger().error(f"Serial port error: {e}") + if self.ser and self.ser.is_open: + self.ser.close() + + def destroy_node(self): + if self.ser and self.ser.is_open: + self.ser.close() + self.get_logger().info("Serial port closed.") + super().destroy_node() + + +def main(args=None): rclpy.init(args=args) node = ArduinoInterfaceNode() - try: rclpy.spin(node) except KeyboardInterrupt: node.get_logger().info("Node stopped by user.") finally: - rclpy.shutdown() \ No newline at end of file + node.destroy_node() + rclpy.shutdown() diff --git a/ros_code/ros_ws/src/pilla_teleop/package.xml b/ros_code/ros_ws/src/pilla_teleop/package.xml index fc7de30..b3f9a81 100644 --- a/ros_code/ros_ws/src/pilla_teleop/package.xml +++ b/ros_code/ros_ws/src/pilla_teleop/package.xml @@ -1,5 +1,6 @@ - + + pilla_teleop 0.6.2 Gamepad teleop node for Pilla Robot. @@ -12,13 +13,13 @@ Vishwajeet Narwal - ament_cmake - geometry_msgs - joy - rclpy - sensor_msgs + champ_msgs + geometry_msgs + joy + rclpy + sensor_msgs ament_cmake