diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 5267ccb..071a71a 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -90,9 +90,9 @@ class SerialRelay(Node): msg.data = output if output.startswith("can_relay_fromvic,core"): self.core_pub.publish(msg) - elif output.startswith("can_relay_fromvic,arm"): + elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"): # digit for voltage readings self.arm_pub.publish(msg) - elif output.startswith("can_relay_fromvic,bio"): + elif output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"): # digit for SHT sensor self.bio_pub.publish(msg) # msg = String() # msg.data = output @@ -135,16 +135,16 @@ def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) if serial_pub: serial_pub.cleanup() - + def main(args=None): - rclpy.init(args=args) - sys.excepthook = myexcepthook + rclpy.init(args=args) + sys.excepthook = myexcepthook - global serial_pub + global serial_pub - serial_pub = SerialRelay() - serial_pub.run() + serial_pub = SerialRelay() + serial_pub.run() if __name__ == '__main__': signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 1523ecd..f4b582f 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -9,6 +9,7 @@ import atexit import signal from std_msgs.msg import String from ros2_interfaces_pkg.msg import BioControl +from ros2_interfaces_pkg.msg import BioFeedback serial_pub = None thread = None @@ -25,10 +26,10 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10) - #self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) + self.bio_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10) - # Create subscribers\ + # Create subscribers self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10) # Topics used in anchor mode @@ -136,7 +137,7 @@ class SerialRelay(Node): self.send_cmd(command) - # LSS + # LSS (SCYTHE) command = "can_relay_tovic,citadel,24," + str(msg.lss_direction) + "\n" self.send_cmd(command) # Vibration Motor @@ -150,18 +151,18 @@ class SerialRelay(Node): # To be reviewed before use# # Laser - command = "can_relay_tovic,faerie,28," + str(msg.laser) + "\n" + command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n" self.send_cmd(command) - - # # UV Light - # command = "can_relay_tovic,faerie,38," + str(msg.uvLight) + "\n" - # self.send_cmd(command) - # Drill - command = f"can_relay_tovic,faerie,19,{msg.drill_duty:.2f}\n" + # Drill (SCABBARD) + command = f"can_relay_tovic,digit,19,{msg.drill_duty:.2f}\n" print(msg.drill_duty) self.send_cmd(command) + # Bio linear actuator + command = "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n" + self.send_cmd(command) + def send_cmd(self, msg): diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index f7f39a6..3db6c94 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit f7f39a635287f242a8feacb1792332c0521fc5a7 +Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470