From 7d6294eadf87e844f3f0bf3b1ebb89c603e684a1 Mon Sep 17 00:00:00 2001 From: Nam Date: Sat, 19 Aug 2023 09:41:54 -0700 Subject: [PATCH] Michelle's timing code --- .../firmware_interface.py | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py b/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py index b202388c8..d1c9aacfe 100644 --- a/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py +++ b/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py @@ -1,5 +1,6 @@ import math import os +import statistics import threading import time @@ -57,7 +58,10 @@ def reconnect_serial_port(self): rospy.logerr_throttle(10, "Unable to connect to any serial port /dev/ttyACM0-10") def firmware_update_loop(self): + num_samples = 100 + frequency_holder = [0] * num_samples while not rospy.is_shutdown(): + start_time = time.time() try: self.reconnect_serial_port() # data format @@ -88,10 +92,10 @@ def firmware_update_loop(self): val = data[i * 2 + 2] | data[i * 2 + 3] << 8 motor_name = self.motor_id_to_name_dict[i] - motor_type = self.motor_mapping[motor_name]['type'] - motor_angle_zero = self.motor_mapping[motor_name]['angle_zero'] - max_angle_bytes = self.motor_types[motor_type]['max_angle_bytes'] - max_angle_radians = self.motor_types[motor_type]['max_angle_degrees'] / 180 * math.pi + motor_type = self.motor_mapping[motor_name]["type"] + motor_angle_zero = self.motor_mapping[motor_name]["angle_zero"] + max_angle_bytes = self.motor_types[motor_type]["max_angle_bytes"] + max_angle_radians = self.motor_types[motor_type]["max_angle_degrees"] / 180 * math.pi motor_angle_zero_radian = motor_angle_zero / 180 * math.pi @@ -125,7 +129,6 @@ def firmware_update_loop(self): imu.linear_acceleration.y = -(ay) * G / ACC_RANGE imu.linear_acceleration.z = -(az) * G / ACC_RANGE - vx = int.from_bytes(imu_data[6:8], byteorder="big", signed=True) / IMU_GY_RANGE vy = int.from_bytes(imu_data[8:10], byteorder="big", signed=True) / IMU_GY_RANGE vz = int.from_bytes(imu_data[10:12], byteorder="big", signed=True) / IMU_GY_RANGE @@ -146,6 +149,11 @@ def firmware_update_loop(self): rospy.logerr_throttle(10, f"Lost connection to serial port {type(ex)} {ex}, retrying...") pass + frequency_holder.append(time.time() - start_time) + frequency_holder = frequency_holder[-num_samples:] + average = statistics.mean(frequency_holder) + rospy.loginfo_throttle(1, f"FW interface average loop period = {average}sec, frequency = {1/average}Hz") + def joint_command_callback(self, joint_state: JointState): try: @@ -162,7 +170,6 @@ def joint_command_callback(self, joint_state: JointState): if time_diff_real < time_diff_message_in: rospy.sleep(time_diff_message_in - time_diff_real) - t1 = time.time() self.reconnect_serial_port() @@ -199,15 +206,15 @@ def joint_command_callback(self, joint_state: JointState): bytes_to_write[2 + id * 2 + 1] = angle_final_bytes_2 pass - t2 = time.time() self.serial.write(bytes_to_write) self.last_motor_publish_time_real = rospy.Time.now() self.last_motor_publish_time = joint_state.header.stamp t3 = time.time() - rospy.loginfo(f"Time Lag : {(rospy.Time.now() - joint_state.header.stamp).to_sec()} Bytes Written: {bytes_to_write} Time Take {t2-t1} {t3-t1}") - + rospy.loginfo( + f"Time Lag : {(rospy.Time.now() - joint_state.header.stamp).to_sec()} Bytes Written: {bytes_to_write} Time Take {t2-t1} {t3-t1}" + ) except Exception as ex: rospy.logerr_throttle(10, f"Lost connection to serial port {ex}, retrying...")