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

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

@@ -360,24 +360,25 @@ class SerialRelay(Node):
# TODO: add len(msg.data) checks to each feedback message # TODO: add len(msg.data) checks to each feedback message
match msg.command_id:
# GNSS # GNSS
if msg.command_id == 48: # GNSS Latitude case 48: # GNSS Latitude
self.gps_state.latitude = float(msg.data[0]) self.gps_state.latitude = float(msg.data[0])
elif msg.command_id == 49: # GNSS Longitude case 49: # GNSS Longitude
self.gps_state.longitude = float(msg.data[0]) self.gps_state.longitude = float(msg.data[0])
elif msg.command_id == 50: # GNSS Satellite count and altitude case 50: # GNSS Satellite count and altitude
self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
self.gps_state.altitude = float(msg.data[1]) self.gps_state.altitude = float(msg.data[1])
self.gps_state.header.stamp = msg.header.stamp self.gps_state.header.stamp = msg.header.stamp
self.gps_pub_.publish(self.gps_state) self.gps_pub_.publish(self.gps_state)
# IMU # IMU
elif msg.command_id == 51: # Gyro x, y, z, and imu calibration case 51: # Gyro x, y, z, and imu calibration
self.feedback_new_state.imu_calib = round(float(msg.data[3])) self.feedback_new_state.imu_calib = round(float(msg.data[3]))
self.imu_state.angular_velocity.x = float(msg.data[0]) self.imu_state.angular_velocity.x = float(msg.data[0])
self.imu_state.angular_velocity.y = float(msg.data[1]) self.imu_state.angular_velocity.y = float(msg.data[1])
self.imu_state.angular_velocity.z = float(msg.data[2]) self.imu_state.angular_velocity.z = float(msg.data[2])
self.imu_state.header.stamp = msg.header.stamp self.imu_state.header.stamp = msg.header.stamp
elif msg.command_id == 52: # Accel x, y, z, heading case 52: # Accel x, y, z, heading
self.imu_state.linear_acceleration.x = float(msg.data[0]) self.imu_state.linear_acceleration.x = float(msg.data[0])
self.imu_state.linear_acceleration.y = float(msg.data[1]) self.imu_state.linear_acceleration.y = float(msg.data[1])
self.imu_state.linear_acceleration.z = float(msg.data[2]) self.imu_state.linear_acceleration.z = float(msg.data[2])
@@ -391,7 +392,7 @@ class SerialRelay(Node):
self.imu_state.header.stamp = msg.header.stamp self.imu_state.header.stamp = msg.header.stamp
self.imu_pub_.publish(self.imu_state) self.imu_pub_.publish(self.imu_state)
# REV Motors # REV Motors
elif msg.command_id == 53: # REV SPARK MAX feedback case 53: # REV SPARK MAX feedback
motorId = round(float(msg.data[0])) motorId = round(float(msg.data[0]))
temp = float(msg.data[1]) / 10.0 temp = float(msg.data[1]) / 10.0
voltage = float(msg.data[2]) / 10.0 voltage = float(msg.data[2]) / 10.0
@@ -418,20 +419,20 @@ class SerialRelay(Node):
self.feedback_new_state.br_motor.header.stamp = msg.header.stamp self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
self.feedback_new_pub_.publish(self.feedback_new_state) self.feedback_new_pub_.publish(self.feedback_new_state)
# Board voltage # Board voltage
elif msg.command_id == 54: # Voltages batt, 12, 5, 3, all * 100 case 54: # Voltages batt, 12, 5, 3, all * 100
self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0 self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0 self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0 self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0 self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
# Baro # Baro
elif msg.command_id == 56: # BMP temperature, altitude, pressure case 56: # BMP temperature, altitude, pressure
self.baro_state.temperature = float(msg.data[0]) self.baro_state.temperature = float(msg.data[0])
self.baro_state.altitude = float(msg.data[1]) self.baro_state.altitude = float(msg.data[1])
self.baro_state.pressure = float(msg.data[2]) self.baro_state.pressure = float(msg.data[2])
self.baro_state.header.stamp = msg.header.stamp self.baro_state.header.stamp = msg.header.stamp
self.baro_pub_.publish(self.baro_state) self.baro_pub_.publish(self.baro_state)
# REV Motors (pos and vel) # REV Motors (pos and vel)
elif msg.command_id == 58: # REV position and velocity case 58: # REV position and velocity
motorId = round(float(msg.data[0])) motorId = round(float(msg.data[0]))
position = float(msg.data[1]) position = float(msg.data[1])
velocity = float(msg.data[2]) velocity = float(msg.data[2])
@@ -460,7 +461,7 @@ class SerialRelay(Node):
joint_state_msg.name = ["br_motor_joint"] joint_state_msg.name = ["br_motor_joint"]
joint_state_msg.header.stamp = msg.header.stamp joint_state_msg.header.stamp = msg.header.stamp
self.joint_state_pub_.publish(joint_state_msg) self.joint_state_pub_.publish(joint_state_msg)
else: case _:
return return