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