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

feat: (arm) add use_old_topics parameter

Browse Source
This commit is contained in:
David Sharpe
2026-03-08 00:19:06 -06:00
parent 667247cac8
commit f23d8c62ff
1 changed files with 31 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -64,28 +64,35 @@ class ArmNode(Node):
self.get_logger().info(f"arm launch_mode is: anchor") # Hey I like the output self.get_logger().info(f"arm launch_mode is: anchor") # Hey I like the output
##################################################
# Parameters
self.declare_parameter("use_old_topics", True)
self.use_old_topics = self.get_parameter("use_old_topics").get_parameter_value().bool_value
################################################## ##################################################
# Old topics # Old topics
# Anchor topics if self.use_old_topics:
self.anchor_sub = self.create_subscription( # Anchor topics
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)
# Create publishers # Create publishers
self.socket_pub = self.create_publisher( self.socket_pub = self.create_publisher(
SocketFeedback, "/arm/feedback/socket", 10 SocketFeedback, "/arm/feedback/socket", 10
) )
self.arm_feedback = SocketFeedback() self.arm_feedback = SocketFeedback()
self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10) self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
self.digit_feedback = DigitFeedback() self.digit_feedback = DigitFeedback()
self.feedback_timer = self.create_timer(0.25, self.publish_feedback) self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
# Create subscribers # Create subscribers
self.man_sub = self.create_subscription( self.man_sub = self.create_subscription(
ArmManual, "/arm/control/manual", self.send_manual, 10 ArmManual, "/arm/control/manual", self.send_manual, 10
) )
################################################### ###################################################
# New topics # New topics
@@ -212,6 +219,7 @@ class ArmNode(Node):
self.anchor_tovic_pub_.publish(digit_cmd) self.anchor_tovic_pub_.publish(digit_cmd)
def send_manual(self, msg: ArmManual): def send_manual(self, msg: ArmManual):
"""TODO: Old"""
axis0 = msg.axis0 axis0 = msg.axis0
axis1 = -1 * msg.axis1 axis1 = -1 * msg.axis1
axis2 = msg.axis2 axis2 = msg.axis2
@@ -236,10 +244,12 @@ class ArmNode(Node):
return return
def send_cmd(self, msg: str): def send_cmd(self, msg: str):
"""TODO: Old"""
output = String(data=msg) output = String(data=msg)
self.anchor_pub.publish(output) self.anchor_pub.publish(output)
def anchor_feedback(self, msg: String): def anchor_feedback(self, msg: String):
"""TODO: Old"""
output = msg.data output = msg.data
if output.startswith("can_relay_fromvic,arm,55"): if output.startswith("can_relay_fromvic,arm,55"):
self.updateAngleFeedback(output) self.updateAngleFeedback(output)
@@ -389,10 +399,12 @@ class ArmNode(Node):
) # Wrist yaw ) # Wrist yaw
def publish_feedback(self): def publish_feedback(self):
"""TODO: Old"""
self.socket_pub.publish(self.arm_feedback) self.socket_pub.publish(self.arm_feedback)
self.digit_pub.publish(self.digit_feedback) self.digit_pub.publish(self.digit_feedback)
def updateAngleFeedback(self, msg: str): def updateAngleFeedback(self, msg: str):
"""TODO: Old"""
# Angle feedbacks, # Angle feedbacks,
# split the msg.data by commas # split the msg.data by commas
parts = msg.split(",") parts = msg.split(",")
@@ -411,6 +423,7 @@ class ArmNode(Node):
self.get_logger().info("Invalid angle feedback input format") self.get_logger().info("Invalid angle feedback input format")
def updateBusVoltage(self, msg: str): def updateBusVoltage(self, msg: str):
"""TODO: Old"""
# Bus Voltage feedbacks # Bus Voltage feedbacks
parts = msg.split(",") parts = msg.split(",")
if len(parts) >= 7: if len(parts) >= 7:
@@ -425,6 +438,7 @@ class ArmNode(Node):
self.get_logger().info("Invalid voltage feedback input format") self.get_logger().info("Invalid voltage feedback input format")
def updateMotorFeedback(self, msg: str): def updateMotorFeedback(self, msg: str):
"""TODO: Old"""
parts = str(msg.strip()).split(",") parts = str(msg.strip()).split(",")
motorId = round(float(parts[3])) motorId = round(float(parts[3]))
temp = float(parts[4]) / 10.0 temp = float(parts[4]) / 10.0