aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-04-20 11:51:16 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix(core): populate missing values in /core/feedback_new

Browse Source
This commit is contained in:
David
2026-03-26 00:40:11 -05:00
parent 3f795bf8ed
commit ea36ce6ef4
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -425,6 +425,7 @@ class CoreNode(Node):
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])
self.feedback_new_state.orientation = float(msg.data[3])
# Deal with quaternion # Deal with quaternion
r = Rotation.from_euler("z", float(msg.data[3]), degrees=True) r = Rotation.from_euler("z", float(msg.data[3]), degrees=True)
q = r.as_quat() q = r.as_quat()
@@ -469,6 +470,7 @@ class CoreNode(Node):
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
self.feedback_new_state.board_voltage.header.stamp = msg.header.stamp
# Baro # Baro
case 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])
@@ -483,7 +485,7 @@ class CoreNode(Node):
velocity = float(msg.data[2]) velocity = float(msg.data[2])
joint_state_msg = ( joint_state_msg = (
JointState() JointState()
) # TODO: not sure if all motors should be in each message or not )
joint_state_msg.position = [ joint_state_msg.position = [
position * (2 * pi) / CORE_GEAR_RATIO position * (2 * pi) / CORE_GEAR_RATIO
] # revolutions to radians ] # revolutions to radians
@@ -511,6 +513,10 @@ class CoreNode(Node):
f"Ignoring REV motor feedback 58 with invalid motorId {motorId}" f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
) )
return return
if motor:
motor.position = position
motor.velocity = velocity
# make the fucking shit work # make the fucking shit work
joint_state_msg.name.append("left_suspension_joint") joint_state_msg.name.append("left_suspension_joint")