From cdc2c7e703d4788e8306eb3df691120b5082f5b5 Mon Sep 17 00:00:00 2001 From: David Date: Thu, 24 Jul 2025 00:07:06 -0500 Subject: [PATCH] refactor: post-comp IK testing --- src/anchor_pkg/anchor_pkg/anchor_node.py | 33 +-- src/arm_pkg/arm_pkg/arm_node.py | 254 ++++++++--------------- src/arm_pkg/arm_pkg/astra_arm.py | 35 ++-- src/arm_pkg/setup.py | 4 +- src/arm_pkg/urdf/arm12.urdf | 2 +- src/bio_pkg/bio_pkg/bio_node.py | 14 +- src/core_pkg/core_pkg/core_node.py | 26 ++- 7 files changed, 138 insertions(+), 230 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 670eefb..edef967 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -62,9 +62,8 @@ class SerialRelay(Node): if self.port is None: self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + time.sleep(1) + sys.exit(1) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) @@ -89,23 +88,9 @@ class SerialRelay(Node): if output: # All output over debug temporarily #self.get_logger().info(f"[MCU] {output}") - if output.startswith("can_relay_fromvic,arm"): - # Publish the message to the arm topic - msg = String() - msg.data = output - self.arm_pub.publish(msg) - elif output.startswith("can_relay_fromvic,core"): - # Publish the message to the core topic - msg = String() - msg.data = output - self.core_pub.publish(msg) - elif output.startswith("can_relay_fromvic,bio"): - # Publish the message to the bio topic - msg = String() - msg.data = output - self.bio_pub.publish(msg) msg = String() msg.data = output + self.debug_pub.publish(msg) if output.startswith("can_relay_fromvic,core"): self.core_pub.publish(msg) elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"): # digit for voltage readings @@ -121,19 +106,19 @@ class SerialRelay(Node): print("Closing serial port.") if self.ser.is_open: self.ser.close() - self.exit(1) + exit(1) except TypeError as e: print(f"TypeError: {e}") print("Closing serial port.") if self.ser.is_open: self.ser.close() - self.exit(1) + exit(1) except Exception as e: print(f"Exception: {e}") - print("Closing serial port.") - if self.ser.is_open: - self.ser.close() - self.exit(1) + # print("Closing serial port.") + # if self.ser.is_open: + # self.ser.close() + # exit(1) def send_cmd(self, msg): message = msg.data diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 230e3a1..9a7c6c0 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -1,5 +1,6 @@ import rclpy from rclpy.node import Node +from rclpy import qos import serial import sys import threading @@ -26,12 +27,21 @@ from ament_index_python.packages import get_package_share_directory from . import astra_arm +control_qos = qos.QoSProfile( + history=qos.QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, + durability=qos.QoSDurabilityPolicy.VOLATILE, + deadline=1000, + lifespan=500, + liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, + liveliness_lease_duration=5000 +) + serial_pub = None thread = None - - class SerialRelay(Node): def __init__(self): # Initialize node @@ -49,7 +59,7 @@ class SerialRelay(Node): self.feedback_timer = self.create_timer(0.25, self.publish_feedback) # Create subscribers - self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) + self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, qos_profile=control_qos) self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10) self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10) @@ -59,20 +69,11 @@ class SerialRelay(Node): self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) - - # output to console - self.get_logger().info("Creating arm object...") - - self.arm_feedback = SocketFeedback() - self.digit_feedback = DigitFeedback() self.arm = astra_arm.Arm('arm12.urdf') self.arm_feedback = SocketFeedback() + self.digit_feedback = DigitFeedback() - ######## - # Interface with MCU # Search for ports IF in 'arm' (standalone) and not 'anchor' mode - ######## - if self.launch_mode == 'arm': # Loop through all serial devices on the computer to check for the MCU self.port = None @@ -84,7 +85,7 @@ class SerialRelay(Node): ser = serial.Serial(port, 115200, timeout=1) #print(f"Checking port {port}...") ser.write(b"ping\n") - response = ser.read_until("\n") + response = ser.read_until("\n") # type: ignore # if pong is in response, then we are talking with the MCU if b"pong" in response: @@ -97,10 +98,9 @@ class SerialRelay(Node): break if self.port is None: - self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + 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) @@ -130,18 +130,9 @@ class SerialRelay(Node): try: output = str(self.ser.readline(), "utf8") if output: - if output.startswith("can_relay_fromvic,arm,55"): - pass - #self.updateAngleFeedback(output) - elif output.startswith("can_relay_fromvic,arm,54"): - pass - #self.updateBusVoltage(output) - elif output.startswith("can_relay_fromvic,arm,53"): - pass - #self.updateMotorFeedback(output) - self.get_logger().info(f"[MCU] {output}") + #self.get_logger().info(f"[MCU] {output}") msg = String() - msg.data = "From MCU Got: " + output + msg.data = output self.debug_pub.publish(msg) except serial.SerialException: self.get_logger().info("SerialException caught... closing serial port.") @@ -162,9 +153,9 @@ class SerialRelay(Node): pass - def updateAngleFeedback(self, msg): + def updateAngleFeedback(self, msg: str): # Angle feedbacks, - #split the msg.data by commas + # split the msg.data by commas parts = msg.split(",") if len(parts) >= 7: @@ -172,7 +163,7 @@ class SerialRelay(Node): angles_in = parts[3:7] # Convert the angles to floats divide by 10.0 angles = [float(angle) / 10.0 for angle in angles_in] - angles[0] = 0.0 #override axis0 to zero + # angles[0] = 0.0 #override axis0 to zero # # #THIS NEEDS TO BE REMOVED LATER @@ -197,7 +188,8 @@ class SerialRelay(Node): else: self.get_logger().info("Invalid angle feedback input format") - def updateBusVoltage(self, msg): + + def updateBusVoltage(self, msg: str): # Bus Voltage feedbacks parts = msg.split(",") if len(parts) >= 7: @@ -210,23 +202,29 @@ class SerialRelay(Node): self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0 else: self.get_logger().info("Invalid voltage feedback input format") - - def updateMotorFeedback(self, msg): - # Motor voltage/current/temperature feedback - parts = msg.split(",") - if len(parts) >= 7: - # Extract the voltage/current/temperature from the string - values_in = parts[3:7] - # Convert the voltages to floats - for i in range(4): - #update arm_feedback's axisX_temp for each axis0_temp, axis1_temp, etc... - pass - - # self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) - # self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) - # self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) - else: - self.get_logger().info("Invalid motor feedback input format") + + def updateMotorFeedback(self, msg: str): + parts = str(msg.strip()).split(",") + motorId = round(float(parts[3])) + temp = float(parts[4]) / 10.0 + voltage = float(parts[5]) / 10.0 + current = float(parts[6]) / 10.0 + if motorId == 1: + self.arm_feedback.axis1_temp = temp + self.arm_feedback.axis1_voltage = voltage + self.arm_feedback.axis1_current = current + elif motorId == 2: + self.arm_feedback.axis2_temp = temp + self.arm_feedback.axis2_voltage = voltage + self.arm_feedback.axis2_current = current + elif motorId == 3: + self.arm_feedback.axis3_temp = temp + self.arm_feedback.axis3_voltage = voltage + self.arm_feedback.axis3_current = current + elif motorId == 4: + self.arm_feedback.axis0_temp = temp + self.arm_feedback.axis0_voltage = voltage + self.arm_feedback.axis0_current = current def send_manual(self, msg: ArmManual): axis0 = msg.axis0 @@ -234,11 +232,6 @@ class SerialRelay(Node): axis2 = msg.axis2 axis3 = msg.axis3 - # tempMsg = String() - # tempMsg.data = "Sending manual" - # self.debug_pub.publish(tempMsg) - - #Send controls for arm command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n" command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n" @@ -293,27 +286,7 @@ class SerialRelay(Node): #pass self.updateBusVoltage(output) elif output.startswith("can_relay_fromvic,arm,53"): - parts = str(output.strip()).split(",") - motorId = round(float(parts[3])) - temp = float(parts[4]) / 10.0 - voltage = float(parts[5]) / 10.0 - current = float(parts[6]) / 10.0 - if motorId == 1: - self.arm_feedback.axis1_temp = temp - self.arm_feedback.axis1_voltage = voltage - self.arm_feedback.axis1_current = current - elif motorId == 2: - self.arm_feedback.axis2_temp = temp - self.arm_feedback.axis2_voltage = voltage - self.arm_feedback.axis2_current = current - elif motorId == 3: - self.arm_feedback.axis3_temp = temp - self.arm_feedback.axis3_voltage = voltage - self.arm_feedback.axis3_current = current - elif motorId == 4: - self.arm_feedback.axis0_temp = temp - self.arm_feedback.axis0_voltage = voltage - self.arm_feedback.axis0_current = current + self.updateMotorFeedback(output) elif output.startswith("can_relay_fromvic,digit,54"): parts = msg.data.split(",") if len(parts) >= 7: @@ -334,80 +307,18 @@ class SerialRelay(Node): self.socket_pub.publish(self.arm_feedback) self.digit_pub.publish(self.digit_feedback) - def updateAngleFeedback(self, msg: str): - # Angle feedbacks, - #split the msg.data by commas - parts = msg.split(",") - - if len(parts) >= 7: - # Extract the angles from the string - angles_in = parts[3:7] - # Convert the angles to floats divide by 10.0 - angles = [float(angle) / 10.0 for angle in angles_in] - #angles[0] = 0.0 #override axis0 to zero - # - # - #THIS NEEDS TO BE REMOVED LATER - #PLACEHOLDER FOR WRIST VALUE - # - # - angles.append(0.0)#placeholder for wrist_continuous - angles.append(0.0)#placeholder for wrist - # - # - # # Update the arm's current angles - #self.arm.update_angles(angles) - self.arm_feedback.axis0_angle = angles[0] - self.arm_feedback.axis1_angle = angles[1] - self.arm_feedback.axis2_angle = angles[2] - self.arm_feedback.axis3_angle = angles[3] - # self.get_logger().info(f"Angles: {angles}") - # #debug publish angles - # tempMsg = String() - # tempMsg.data = "Angles: " + str(angles) - # #self.debug_pub.publish(tempMsg) - else: - self.get_logger().info("Invalid angle feedback input format") - - - def updateBusVoltage(self, msg: str): - # Bus Voltage feedbacks - parts = msg.split(",") - if len(parts) >= 7: - # Extract the voltage from the string - voltages_in = parts[3:7] - # Convert the voltages to floats - self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0 - self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0 - self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0 - self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0 - else: - self.get_logger().info("Invalid voltage feedback input format") - - def publish_feedback(self): - # Create a SocketFeedback message and publish it - # msg = SocketFeedback() - # msg.bat_voltage = self.arm_feedback.bat_voltage - # msg.voltage_12 = self.arm_feedback.voltage_12 - # msg.voltage_5 = self.arm_feedback.voltage_5 - # msg.voltage_3 = self.arm_feedback.voltage_3 - # msg.joint_angles = self.arm_feedback.joint_angles - # msg.joint_temps = self.arm_feedback.joint_temps - # msg.joint_voltages = self.arm_feedback.joint_voltages - # msg.joint_currents = self.arm_feedback.joint_currents - #debug print - self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm - - #self.arm.update_position() #Run FK and update the current position of the arm, using FK - - - def send_ik(self, msg): # Convert Vector3 to a NumPy array - input_raw = np.array([msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z]) # Convert input to a NumPy array + input_raw = np.array([-msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z]) # Convert input to a NumPy array # decrease input vector by 90% input_raw = input_raw * 0.2 + if input_raw[0] == 0.0 and input_raw[1] == 0.0 and input_raw[2] == 0.0: + self.get_logger().info("No input, stopping arm.") + command = "can_relay_tovic,arm,39,0,0,0,0\n" + self.send_cmd(command) + return + # Debug output tempMsg = String() tempMsg.data = "From IK Control Got Vector: " + str(input_raw) @@ -420,7 +331,8 @@ class SerialRelay(Node): #Print for IK DEBUG tempMsg = String() - tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles) + # tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles) + tempMsg.data = "Current Angles: " + str(math.degrees(self.arm.current_angles[2])) + ", " + str(math.degrees(self.arm.current_angles[4])) + ", " + str(math.degrees(self.arm.current_angles[6])) self.ik_debug.publish(tempMsg) self.get_logger().info(f"[IK] {tempMsg.data}") @@ -433,48 +345,41 @@ class SerialRelay(Node): #self.debug_pub.publish(tempMsg) # Perform IK with the target position - if self.arm.perform_ik(target_position): + if self.arm.perform_ik(target_position, self.get_logger()): # Send command to control #command = "can_relay_tovic,arm,32," + ",".join(map(str, self.arm.ik_angles[:4])) + "\n" #self.send_cmd(command) self.get_logger().info(f"IK Success: {target_position}") + self.get_logger().info(f"IK Angles: [{str(round(math.degrees(self.arm.ik_angles[2]), 2))}, {str(round(math.degrees(self.arm.ik_angles[4]), 2))}, {str(round(math.degrees(self.arm.ik_angles[6]), 2))}]") # tempMsg = String() # tempMsg.data = "IK Success: " + str(target_position) # #self.debug_pub.publish(tempMsg) # tempMsg.data = "Sending: " + str(command) #self.debug_pub.publish(tempMsg) + + # Send the IK angles to the MCU + command = "can_relay_tovic,arm,32," + f"{math.degrees(self.arm.ik_angles[0])*10},{math.degrees(self.arm.ik_angles[2])*10},{math.degrees(self.arm.ik_angles[4])*10},{math.degrees(self.arm.ik_angles[6])*10}" + "\n" + # self.send_cmd(command) + + # Manual control for Wrist/Effector + command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" + + command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n" + + command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n" + + command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n" + + self.send_cmd(command) else: self.get_logger().info("IK Fail") + self.get_logger().info(f"IK Angles: [{str(math.degrees(self.arm.ik_angles[2]))}, {str(math.degrees(self.arm.ik_angles[4]))}, {str(math.degrees(self.arm.ik_angles[6]))}]") # tempMsg = String() # tempMsg.data = "IK Fail" #self.debug_pub.publish(tempMsg) - # Manual control for Wrist/Effector - command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" - self.send_cmd(command) - - command = "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n" - self.send_cmd(command) - - command = "can_relay_tovic,digit,26," + str(msg.gripper) + "\n" - self.send_cmd(command) - - command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n" - self.send_cmd(command) - - # Placeholder need control for linear actuator - #command = "" - #self.send_cmd() - - - - pass - - - - @staticmethod def list_serial_ports(): @@ -483,8 +388,11 @@ class SerialRelay(Node): def cleanup(self): print("Cleaning up...") - if self.ser.is_open: - self.ser.close() + try: + if self.ser.is_open: + self.ser.close() + except Exception as e: + exit(0) def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index a61bd3a..44a1373 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -1,3 +1,5 @@ +import rclpy +from rclpy.node import Node import numpy as np import time, math, os from math import sin, cos, pi @@ -15,16 +17,16 @@ degree = pi / 180.0 def convert_angles(angles): # Converts angles to the format used for the urdf (contains some dummy joints) - return [0.0, angles[0]*degree, angles[1]*degree, 0.0, angles[2]*degree, 0.0, angles[3]*degree, 0.0, angles[4]*degree, angles[5]*degree, 0.0] + return [0.0, math.radians(angles[0]), math.radians(angles[1]), 0.0, math.radians(angles[2]), 0.0, math.radians(angles[3]), 0.0, math.radians(angles[4]), math.radians(angles[5]), 0.0] class Arm: def __init__(self, urdf_name): - self.ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid + self.ik_tolerance = 1e-1 #Tolerance (in meters) to determine if solution is valid # URDF file path - self.urdf = os.path.join(get_package_share_directory('arm_pkg'), urdf_name) + self.urdf = os.path.join(get_package_share_directory('arm_pkg'), 'urdf', urdf_name) # IKpy Chain - self.chain = Chain.from_urdf_file(self.urdf) + self.chain = Chain.from_urdf_file(self.urdf) # Arrays for joint states @@ -35,16 +37,16 @@ class Arm: self.last_angles = self.zero_angles self.ik_angles = self.zero_angles - self.current_position = [] + self.current_position: list[float] = [] self.target_position = [0.0, 0.0, 0.0] - self.target_orientation = [] # Effector orientation desired at target position. + self.target_orientation: list = [] # Effector orientation desired at target position. # Generally orientation for the effector is modified manually by the operator. # Might not need, copied over from state_publisher.py in ik_test #self.step = 0.03 # Max movement increment - def perform_ik(self, target_position): + def perform_ik(self, target_position, logger): self.target_position = target_position # Update the target orientation to the current orientation self.update_orientation() @@ -62,19 +64,19 @@ class Arm: # Check if the solution is within the tolerance fk_matrix = self.chain.forward_kinematics(self.ik_angles) - fk_position = fk_matrix[:3, 3] + fk_position = fk_matrix[:3, 3] # type: ignore # print(f"[TRY] FK Position for Solution: {fk_position}") error = np.linalg.norm(target_position - fk_position) if error > self.ik_tolerance: - print(f"No VALID IK Solution within tolerance. Error: {error}") + logger.info(f"No VALID IK Solution within tolerance. Error: {error}") return False else: - print(f"IK Solution Found. Error: {error}") + logger.info(f"IK Solution Found. Error: {error}") return True except Exception as e: - print(f"IK failed for exception: {e}") + logger.info(f"IK failed for exception: {e}") return False # # Given the FK_Matix for the arm's current pose, update the orientation array @@ -93,7 +95,7 @@ class Arm: fk_matrix = self.chain.forward_kinematics(self.current_angles) # Update target_orientation to the effector's current orientation - self.target_orientation = fk_matrix[:3, :3] + self.target_orientation = fk_matrix[:3, :3] # type: ignore # Update current angles to those provided # Resetting last_angles to the new angles @@ -119,7 +121,7 @@ class Arm: fk_matrix = self.chain.forward_kinematics(self.current_angles) # Get the position of the end effector from the FK matrix - position = fk_matrix[:3, 3] + position = fk_matrix[:3, 3] # type: ignore return position @@ -129,7 +131,7 @@ class Arm: fk_matrix = self.chain.forward_kinematics(self.current_angles) # Get the position of the end effector from the FK matrix - position = fk_matrix[:3, 3] + position = fk_matrix[:3, 3] # type: ignore # Return position as a NumPy array return np.array(position) @@ -140,7 +142,4 @@ class Arm: fk_matrix = self.chain.forward_kinematics(self.current_angles) # Get the position of the end effector from the FK matrix and update current pos - self.current_position = fk_matrix[:3, 3] - - - + self.current_position = fk_matrix[:3, 3] # type: ignore diff --git a/src/arm_pkg/setup.py b/src/arm_pkg/setup.py index 6d55985..089982a 100644 --- a/src/arm_pkg/setup.py +++ b/src/arm_pkg/setup.py @@ -12,8 +12,8 @@ setup( ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), - (os.path.join('share', package_name), glob('urdf/*')), + (os.path.join('share', package_name, 'launch'), glob('launch/*')), + (os.path.join('share', package_name, 'urdf'), glob('urdf/*')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/arm_pkg/urdf/arm12.urdf b/src/arm_pkg/urdf/arm12.urdf index 6adb724..b63d872 100644 --- a/src/arm_pkg/urdf/arm12.urdf +++ b/src/arm_pkg/urdf/arm12.urdf @@ -115,7 +115,7 @@ - + diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 5d6fa7a..94b4fba 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -66,10 +66,9 @@ class SerialRelay(Node): pass if self.port is None: - self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + 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) @@ -204,8 +203,11 @@ class SerialRelay(Node): def cleanup(self): print("Cleaning up...") - if self.ser.is_open: - self.ser.close() + try: + if self.ser.is_open: + self.ser.close() + except Exception as e: + exit(0) def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index ac50a69..9004705 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -1,5 +1,6 @@ import rclpy from rclpy.node import Node +from rclpy import qos from std_srvs.srv import Empty import signal @@ -19,6 +20,17 @@ from ros2_interfaces_pkg.msg import CoreControl serial_pub = None thread = None +control_qos = qos.QoSProfile( + history=qos.QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, + durability=qos.QoSDurabilityPolicy.VOLATILE, + deadline=1000, + lifespan=500, + liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, + liveliness_lease_duration=5000 +) + class SerialRelay(Node): def __init__(self): # Initalize node with name @@ -33,7 +45,7 @@ class SerialRelay(Node): self.debug_pub = self.create_publisher(String, '/core/debug', 10) self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10) # Create a subscriber - self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) + self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, qos_profile=control_qos) # Create a publisher for telemetry self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback) @@ -75,9 +87,8 @@ class SerialRelay(Node): if self.port is None: self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + time.sleep(1) + sys.exit(1) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) @@ -265,8 +276,11 @@ class SerialRelay(Node): def cleanup(self): print("Cleaning up before terminating...") - if self.ser.is_open: - self.ser.close() + try: + if self.ser.is_open: + self.ser.close() + except Exception as e: + exit(0) def myexcepthook(type, value, tb): print("Uncaught exception:", type, value)