diff --git a/flake.nix b/flake.nix index 17dd3b9..b6df787 100644 --- a/flake.nix +++ b/flake.nix @@ -39,6 +39,7 @@ buildEnv { paths = [ ros-core + rqt-graph ros2cli ros2run ros2bag diff --git a/src/astra_msgs b/src/astra_msgs index 6a57072..ea0a680 160000 --- a/src/astra_msgs +++ b/src/astra_msgs @@ -1 +1 @@ -Subproject commit 6a5707272398e364005ca2bfa17bea648f704b49 +Subproject commit ea0a680d2261a3579704fd2b7d075e831fee010e diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index fce0b6e..d24546a 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -1,20 +1,29 @@ -import rclpy -from rclpy.node import Node -import serial +import signal import sys import threading -import os -import glob import time -import atexit -import signal -from std_msgs.msg import String -from astra_msgs.msg import BioControl -from astra_msgs.msg import BioFeedback + +import rclpy +from rclpy.action import ActionServer +from rclpy.node import Node +from std_msgs.msg import Header, String + +from astra_msgs.action import BioVacuum +from astra_msgs.msg import BioControl, BioDistributor, BioFeedback, VicCAN +from astra_msgs.srv import BioTestTube serial_pub = None thread = None +# used to verify the length of an incoming VicCAN feedback message +# key is VicCAN command_id, value is expected length of data list +viccan_citadel_msg_len_dict = { + # empty because not expecting any VicCAN from citadel atm +} +viccan_lance_msg_len_dict = { + # empty because not sure what VicCAN commands LANCE sends +} + class SerialRelay(Node): def __init__(self): @@ -22,172 +31,55 @@ class SerialRelay(Node): super().__init__("bio_node") # Get launch mode parameter - self.declare_parameter("launch_mode", "bio") + self.declare_parameter("launch_mode") self.launch_mode = self.get_parameter("launch_mode").value self.get_logger().info(f"bio launch_mode is: {self.launch_mode}") - # Create publishers - self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10) - self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10) - - # Create subscribers - self.control_sub = self.create_subscription( - BioControl, "/bio/control", self.send_control, 10 - ) - - # Create a publisher for telemetry - self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback) - - # Topics used in anchor mode + # Anchor Topics if self.launch_mode == "anchor": + self.anchor_fromvic_sub_ = self.create_subscription( + VicCAN, "/anchor/from_vic/bio", self.relay_fromvic, 20 + ) + self.anchor_tovic_pub_ = self.create_publisher( + VicCAN, "/anchor/to_vic/relay", 20 + ) + self.anchor_sub = self.create_subscription( String, "/anchor/bio/feedback", self.anchor_feedback, 10 ) self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10) - self.bio_feedback = BioFeedback() + self.distributor_sub = self.create_subscription( + BioDistributor, + "/bio/control/distributor", + self.distributor_callback, + 10, + ) - # Search for ports IF in 'arm' (standalone) and not 'anchor' mode - if self.launch_mode == "bio": - # Loop through all serial devices on the computer to check for the MCU - self.port = None - for i in range(2): - try: - # connect and send a ping command - set_port = ( - "/dev/ttyACM0" # MCU is controlled through GPIO pins on the PI - ) - ser = serial.Serial(set_port, 115200, timeout=1) - # print(f"Checking port {port}...") - ser.write(b"ping\n") - response = ser.read_until("\n") + # Services - # if pong is in response, then we are talking with the MCU - if b"pong" in response: - self.port = set_port - self.get_logger().info(f"Found MCU at {set_port}!") - break - except: - pass + self.test_tube_service = self.create_service( + BioTestTube, "/bio/control/test_tube", self.test_tube_callback + ) - if self.port is None: - self.get_logger().info( - "Unable to find MCU... please make sure it is connected." - ) - time.sleep(1) - sys.exit(1) - - self.ser = serial.Serial(self.port, 115200) - atexit.register(self.cleanup) + # Actions + self._action_server = ActionServer( + self, BioVacuum, "/bio/actions/vacuum", self.execute_vacuum + ) def run(self): global thread thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True) thread.start() - # if in arm mode, will need to read from the MCU - try: while rclpy.ok(): - if self.launch_mode == "bio": - if self.ser.in_waiting: - self.read_mcu() - else: - time.sleep(0.1) + time.sleep(0.1) except KeyboardInterrupt: pass - finally: - self.cleanup() - - # Currently will just spit out all values over the /arm/feedback/debug topic as strings - def read_mcu(self): - try: - output = str(self.ser.readline(), "utf8") - if output: - self.get_logger().info(f"[MCU] {output}") - msg = String() - msg.data = output - self.debug_pub.publish(msg) - except serial.SerialException: - self.get_logger().info("SerialException caught... closing serial port.") - if self.ser.is_open: - self.ser.close() - pass - except TypeError as e: - self.get_logger().info(f"TypeError: {e}") - print("Closing serial port.") - if self.ser.is_open: - self.ser.close() - pass - except Exception as e: - print(f"Exception: {e}") - print("Closing serial port.") - if self.ser.is_open: - self.ser.close() - pass - - def send_ik(self, msg): - pass def send_control(self, msg: BioControl): - # CITADEL Control Commands - ################ - - # Chem Pumps, only send if not zero - if msg.pump_id != 0: - command = ( - "can_relay_tovic,citadel,27," - + str(msg.pump_id) - + "," - + str(msg.pump_amount) - + "\n" - ) - self.send_cmd(command) - # Fans, only send if not zero - if msg.fan_id != 0: - command = ( - "can_relay_tovic,citadel,40," - + str(msg.fan_id) - + "," - + str(msg.fan_duration) - + "\n" - ) - self.send_cmd(command) - # Servos, only send if not zero - if msg.servo_id != 0: - command = ( - "can_relay_tovic,citadel,25," - + str(msg.servo_id) - + "," - + str(int(msg.servo_state)) - + "\n" - ) - self.send_cmd(command) - - # LSS (SCYTHE) - command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n" - # self.send_cmd(command) - - # Vibration Motor - command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n" - # self.send_cmd(command) - - # FAERIE Control Commands - ################ - - # To be reviewed before use# - - # Laser - command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n" - # self.send_cmd(command) - - # Drill (SCABBARD) - command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n" - # self.send_cmd(command) - - # Bio linear actuator - command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n" - self.send_cmd(command) + print("todo") def send_cmd(self, msg: str): if ( @@ -196,46 +88,129 @@ class SerialRelay(Node): output = String() output.data = msg self.anchor_pub.publish(output) - elif self.launch_mode == "bio": # if in standalone mode, send to MCU directly - self.get_logger().info(f"[Bio to MCU] {msg}") - self.ser.write(bytes(msg, "utf8")) + + def relay_fromvic(self, msg: VicCAN): + # self.get_logger().info(msg) + if msg.mcu_name == "citadel": + self.process_fromvic_citadel(msg) + + def process_fromvic_citadel(self, msg: VicCAN): + if msg.mcu_name != "citadel": + return + + # Check message len to prevent crashing on bad data + if msg.command_id in viccan_citadel_msg_len_dict: + expected_len = viccan_citadel_msg_len_dict[msg.command_id] + if len(msg.data) != expected_len: + self.get_logger().warning( + f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})" + ) + return def anchor_feedback(self, msg: String): output = msg.data parts = str(output.strip()).split(",") - # self.get_logger().info(f"[Bio Anchor] {msg.data}") + self.get_logger().info(f"[Bio Anchor] {msg.data}") + # no data planned to be received from citadel, not sure about lance - 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.voltage_12 = float(parts[4]) / 100.0 - self.bio_feedback.voltage_5 = float(parts[5]) / 100.0 - elif output.startswith("can_relay_fromvic,digit,57"): - self.bio_feedback.drill_temp = float(parts[3]) - self.bio_feedback.drill_humidity = float(parts[4]) + def distributor_callback(self, msg: BioDistributor): + distributor_arr = msg.distributor_id + vic_cmd = VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="citadel", + command_id=40, + data=[ + to_short(distributor_arr[0]), + to_short(distributor_arr[1]), + to_short(distributor_arr[2]), + 0, + ], + ) + self.anchor_tovic_pub_.publish(vic_cmd) - def publish_feedback(self): - self.feedback_pub.publish(self.bio_feedback) + def test_tube_callback(self, request, response): + tubes_cmd = VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="citadel", + command_id=40, + data=[ + float(int.from_bytes(request.tube_id)), + float(request.milliliters), + ], + ) + self.anchor_tovic_pub_.publish(tubes_cmd) + return response - @staticmethod - def list_serial_ports(): - return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*") - # return glob.glob("/dev/tty[A-Za-z]*") + def execute_vacuum(self, goal_handle): + valve_id = int.from_bytes(goal_handle.request.valve_id) + duty = int.from_bytes(goal_handle.request.fan_duty_cycle_percent) + total = goal_handle.request.fan_time_ms - def cleanup(self): - print("Cleaning up...") - try: - if self.ser.is_open: - self.ser.close() - except Exception as e: - exit(0) + # open valve + self.anchor_tovic_pub_.publish( + VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="citadel", + command_id=40, + data=[float(valve_id)], + ) + ) + + # set fan duty cycle + self.anchor_tovic_pub_.publish( + VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="citadel", + command_id=19, + data=[float(duty)], + ) + ) + + feedback = BioVacuum.Feedback() + start = time.time() + + while True: + elapsed = int((time.time() - start) * 1000) + remaining = max(0, total - elapsed) + + feedback.fan_time_remaining_ms = remaining + goal_handle.publish_feedback(feedback) + + if remaining == 0: + break + + time.sleep(0.1) + + # stop fan + self.anchor_tovic_pub_.publish( + VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="citadel", + command_id=19, + data=[0.0], + ) + ) + + # close valve + self.anchor_tovic_pub_.publish( + VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="citadel", + command_id=40, + data=[-1.0], + ) + ) + + goal_handle.succeed() + return BioVacuum.Result() + + +def to_short(x: int) -> int: + return max(-32768, min(32767, x)) def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) - if serial_pub: - serial_pub.cleanup() def main(args=None):