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

feat: actually add digit feedback

Browse Source
This commit is contained in:
David
2025-05-23 22:42:12 +00:00
parent 02b8904d63
commit 76ca9d81b2
3 changed files with 16 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -10,7 +10,8 @@ import signal
from std_msgs.msg import String from std_msgs.msg import String
from ros2_interfaces_pkg.msg import ArmManual from ros2_interfaces_pkg.msg import ArmManual
from ros2_interfaces_pkg.msg import ArmIK from ros2_interfaces_pkg.msg import ArmIK
from ros2_interfaces_pkg.msg import ArmFeedback from ros2_interfaces_pkg.msg import SocketFeedback
from ros2_interfaces_pkg.msg import DigitFeedback
serial_pub = None serial_pub = None
thread = None thread = None
@@ -27,7 +28,8 @@ class SerialRelay(Node):
# Create publishers # Create publishers
self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10) self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10)
self.socket_pub = self.create_publisher(ArmFeedback, '/arm/feedback/socket', 10) self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10)
self.digit_pub = self.create_publisher(DigitFeedback, '/arm/feedback/digit', 10)
self.feedback_timer = self.create_timer(1.0, self.publish_feedback) self.feedback_timer = self.create_timer(1.0, self.publish_feedback)
# Create subscribers # Create subscribers
@@ -39,7 +41,8 @@ class SerialRelay(Node):
self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
self.arm_feedback = ArmFeedback() self.arm_feedback = SocketFeedback()
self.digit_feedback = DigitFeedback()
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
if self.launch_mode == 'arm': if self.launch_mode == 'arm':
@@ -188,15 +191,19 @@ class SerialRelay(Node):
# Extract the voltage from the string # Extract the voltage from the string
voltages_in = parts[3:7] voltages_in = parts[3:7]
# Convert the voltages to floats # Convert the voltages to floats
self.arm_feedback.digit_bat_voltage = float(voltages_in[0]) / 100.0 self.digit_feedback.bat_voltage = float(voltages_in[0]) / 100.0
self.arm_feedback.digit_voltage_12 = float(voltages_in[1]) / 100.0 self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0
self.arm_feedback.digit_voltage_5 = float(voltages_in[2]) / 100.0 self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0
self.arm_feedback.digit_voltage_3 = float(voltages_in[3]) / 100.0 elif output.startswith("can_relay_fromvic,digit,55"):
parts = msg.split(",")
if len(parts) >= 4:
self.digit_feedback.wrist_angle = float(parts[3])
else: else:
return return
def publish_feedback(self): def publish_feedback(self):
self.socket_pub.publish(self.arm_feedback) self.socket_pub.publish(self.arm_feedback)
self.digit_pub.publish(self.digit_feedback)
def updateAngleFeedback(self, msg): def updateAngleFeedback(self, msg):
# Angle feedbacks, # Angle feedbacks,

2
src/bio_pkg/bio_pkg/bio_node.py
View File

@@ -184,7 +184,7 @@ class SerialRelay(Node):
parts = str(output.strip()).split(",") parts = str(output.strip()).split(",")
self.get_logger().info(f"[Bio Anchor] {msg.data}") self.get_logger().info(f"[Bio Anchor] {msg.data}")
if output.startswith("can_relay_fromvic,citadel,54"):#bat, 12, 5, 3, Voltage readings * 100 if output.startswith("can_relay_fromvic,citadel,54"): # bat, 12, 5, Voltage readings * 100
self.bio_feedback.bat_voltage = float(parts[3]) / 100.0 self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
self.bio_feedback.voltage_12 = float(parts[4]) / 100.0 self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
self.bio_feedback.voltage_5 = float(parts[5]) / 100.0 self.bio_feedback.voltage_5 = float(parts[5]) / 100.0

2
src/ros2_interfaces_pkg

Submodule src/ros2_interfaces_pkg updated: fac196b5d6...3db6c94833