aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

refactor: change msg.command_id from if elif to match case

Browse Source
This commit is contained in:
David
2025-10-14 13:49:53 -05:00
parent 2d258b3103
commit d565dbe31f
1 changed files with 103 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

205
src/core_pkg/core_pkg/core_node.py
View File

@@ -360,108 +360,109 @@ class SerialRelay(Node):
# TODO: add len(msg.data) checks to each feedback message # TODO: add len(msg.data) checks to each feedback message
# GNSS match msg.command_id:
if msg.command_id == 48: # GNSS Latitude # GNSS
self.gps_state.latitude = float(msg.data[0]) case 48: # GNSS Latitude
elif msg.command_id == 49: # GNSS Longitude self.gps_state.latitude = float(msg.data[0])
self.gps_state.longitude = float(msg.data[0]) case 49: # GNSS Longitude
elif msg.command_id == 50: # GNSS Satellite count and altitude self.gps_state.longitude = float(msg.data[0])
self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX case 50: # GNSS Satellite count and altitude
self.gps_state.altitude = float(msg.data[1]) self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
self.gps_state.header.stamp = msg.header.stamp self.gps_state.altitude = float(msg.data[1])
self.gps_pub_.publish(self.gps_state) self.gps_state.header.stamp = msg.header.stamp
# IMU self.gps_pub_.publish(self.gps_state)
elif msg.command_id == 51: # Gyro x, y, z, and imu calibration # IMU
self.feedback_new_state.imu_calib = round(float(msg.data[3])) case 51: # Gyro x, y, z, and imu calibration
self.imu_state.angular_velocity.x = float(msg.data[0]) self.feedback_new_state.imu_calib = round(float(msg.data[3]))
self.imu_state.angular_velocity.y = float(msg.data[1]) self.imu_state.angular_velocity.x = float(msg.data[0])
self.imu_state.angular_velocity.z = float(msg.data[2]) self.imu_state.angular_velocity.y = float(msg.data[1])
self.imu_state.header.stamp = msg.header.stamp self.imu_state.angular_velocity.z = float(msg.data[2])
elif msg.command_id == 52: # Accel x, y, z, heading self.imu_state.header.stamp = msg.header.stamp
self.imu_state.linear_acceleration.x = float(msg.data[0]) case 52: # Accel x, y, z, heading
self.imu_state.linear_acceleration.y = float(msg.data[1]) self.imu_state.linear_acceleration.x = float(msg.data[0])
self.imu_state.linear_acceleration.z = float(msg.data[2]) self.imu_state.linear_acceleration.y = float(msg.data[1])
# Deal with quaternion self.imu_state.linear_acceleration.z = float(msg.data[2])
r = Rotation.from_euler('z', float(msg.data[3]), degrees=True) # Deal with quaternion
q = r.as_quat() r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
self.imu_state.orientation.x = q[0] q = r.as_quat()
self.imu_state.orientation.y = q[1] self.imu_state.orientation.x = q[0]
self.imu_state.orientation.z = q[2] self.imu_state.orientation.y = q[1]
self.imu_state.orientation.w = q[3] self.imu_state.orientation.z = q[2]
self.imu_state.header.stamp = msg.header.stamp self.imu_state.orientation.w = q[3]
self.imu_pub_.publish(self.imu_state) self.imu_state.header.stamp = msg.header.stamp
# REV Motors self.imu_pub_.publish(self.imu_state)
elif msg.command_id == 53: # REV SPARK MAX feedback # REV Motors
motorId = round(float(msg.data[0])) case 53: # REV SPARK MAX feedback
temp = float(msg.data[1]) / 10.0 motorId = round(float(msg.data[0]))
voltage = float(msg.data[2]) / 10.0 temp = float(msg.data[1]) / 10.0
current = float(msg.data[3]) / 10.0 voltage = float(msg.data[2]) / 10.0
if motorId == 1: current = float(msg.data[3]) / 10.0
self.feedback_new_state.fl_motor.temperature = temp if motorId == 1:
self.feedback_new_state.fl_motor.voltage = voltage self.feedback_new_state.fl_motor.temperature = temp
self.feedback_new_state.fl_motor.current = current self.feedback_new_state.fl_motor.voltage = voltage
self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp self.feedback_new_state.fl_motor.current = current
elif motorId == 2: self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
self.feedback_new_state.bl_motor.temperature = temp elif motorId == 2:
self.feedback_new_state.bl_motor.voltage = voltage self.feedback_new_state.bl_motor.temperature = temp
self.feedback_new_state.bl_motor.current = current self.feedback_new_state.bl_motor.voltage = voltage
self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp self.feedback_new_state.bl_motor.current = current
elif motorId == 3: self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
self.feedback_new_state.fr_motor.temperature = temp elif motorId == 3:
self.feedback_new_state.fr_motor.voltage = voltage self.feedback_new_state.fr_motor.temperature = temp
self.feedback_new_state.fr_motor.current = current self.feedback_new_state.fr_motor.voltage = voltage
self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp self.feedback_new_state.fr_motor.current = current
elif motorId == 4: self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
self.feedback_new_state.br_motor.temperature = temp elif motorId == 4:
self.feedback_new_state.br_motor.voltage = voltage self.feedback_new_state.br_motor.temperature = temp
self.feedback_new_state.br_motor.current = current self.feedback_new_state.br_motor.voltage = voltage
self.feedback_new_state.br_motor.header.stamp = msg.header.stamp self.feedback_new_state.br_motor.current = current
self.feedback_new_pub_.publish(self.feedback_new_state) self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
# Board voltage self.feedback_new_pub_.publish(self.feedback_new_state)
elif msg.command_id == 54: # Voltages batt, 12, 5, 3, all * 100 # Board voltage
self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0 case 54: # Voltages batt, 12, 5, 3, all * 100
self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0 self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0 self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0 self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
# Baro self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
elif msg.command_id == 56: # BMP temperature, altitude, pressure # Baro
self.baro_state.temperature = float(msg.data[0]) case 56: # BMP temperature, altitude, pressure
self.baro_state.altitude = float(msg.data[1]) self.baro_state.temperature = float(msg.data[0])
self.baro_state.pressure = float(msg.data[2]) self.baro_state.altitude = float(msg.data[1])
self.baro_state.header.stamp = msg.header.stamp self.baro_state.pressure = float(msg.data[2])
self.baro_pub_.publish(self.baro_state) self.baro_state.header.stamp = msg.header.stamp
# REV Motors (pos and vel) self.baro_pub_.publish(self.baro_state)
elif msg.command_id == 58: # REV position and velocity # REV Motors (pos and vel)
motorId = round(float(msg.data[0])) case 58: # REV position and velocity
position = float(msg.data[1]) motorId = round(float(msg.data[0]))
velocity = float(msg.data[2]) position = float(msg.data[1])
joint_state_msg = JointState() # TODO: not sure if all motors should be in each message or not velocity = float(msg.data[2])
joint_state_msg.position = [position * (2 * pi) / CORE_GEAR_RATIO] # revolutions to radians joint_state_msg = JointState() # TODO: not sure if all motors should be in each message or not
joint_state_msg.velocity = [velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO] # RPM to rad/s joint_state_msg.position = [position * (2 * pi) / CORE_GEAR_RATIO] # revolutions to radians
if motorId == 1: joint_state_msg.velocity = [velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO] # RPM to rad/s
self.feedback_new_state.fl_motor.position = position if motorId == 1:
self.feedback_new_state.fl_motor.velocity = velocity self.feedback_new_state.fl_motor.position = position
self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp self.feedback_new_state.fl_motor.velocity = velocity
joint_state_msg.name = ["fl_motor_joint"] self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
elif motorId == 2: joint_state_msg.name = ["fl_motor_joint"]
self.feedback_new_state.bl_motor.position = position elif motorId == 2:
self.feedback_new_state.bl_motor.velocity = velocity self.feedback_new_state.bl_motor.position = position
self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp self.feedback_new_state.bl_motor.velocity = velocity
joint_state_msg.name = ["bl_motor_joint"] self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
elif motorId == 3: joint_state_msg.name = ["bl_motor_joint"]
self.feedback_new_state.fr_motor.position = position elif motorId == 3:
self.feedback_new_state.fr_motor.velocity = velocity self.feedback_new_state.fr_motor.position = position
self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp self.feedback_new_state.fr_motor.velocity = velocity
joint_state_msg.name = ["fr_motor_joint"] self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
elif motorId == 4: joint_state_msg.name = ["fr_motor_joint"]
self.feedback_new_state.br_motor.position = position elif motorId == 4:
self.feedback_new_state.br_motor.velocity = velocity self.feedback_new_state.br_motor.position = position
self.feedback_new_state.br_motor.header.stamp = msg.header.stamp self.feedback_new_state.br_motor.velocity = velocity
joint_state_msg.name = ["br_motor_joint"] self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
joint_state_msg.header.stamp = msg.header.stamp joint_state_msg.name = ["br_motor_joint"]
self.joint_state_pub_.publish(joint_state_msg) joint_state_msg.header.stamp = msg.header.stamp
else: self.joint_state_pub_.publish(joint_state_msg)
return case _:
return
def publish_feedback(self): def publish_feedback(self):